
Calhoun: The NPS Institutional Archive 
DSpace Repository 


Theses and Dissertations 


1. Thesis and Dissertation Collection, all items 


1999-03 

Design, implementation, and testing of a 
real-time software system for a 
quaternion-based attitude estimation filter 


Duman, lldeniz. 

Monterey, California: Naval Postgraduate School 


http://hdl.handle.net/10945/13599 


Downloaded from NPS Archive: Calhoun 



DUDLEY 

KNOX 

LIBRARY 


http://www.nps.edu/ljbrary 


CsMwun is the Naval Postgraduate School's public access distal repository for 
research mate riels and tnstitutjiooal pubftcatiions created by the NPS community. 
Cathoufii is named for Professor of Mathematcs Guy K. CatHiuo, NPS's first 
appoinited — and publi^d — scholar^ author. 

Dudley Knox Library / Naval Postgraduate School 
411 Dyer Road / 1 University Circle 
MontereVr California USA 93943 



NAVAL POSTGRADUATE SCHOOL 
Monterey, California 



THESIS 


DESIGN, IMPLEMENTATION, AND TESTING OF A 
REAL-TIME SOFT>VARE SYSTEM FOR A 
QUATERNION-BASED ATTITUDE ESTIMATION FILTER 

by 

Ildeniz Duman 
March 1999 


Thesis Co-Advisors: Eric R. Bachmann 

Robert B. McGhee 

Approved for public release; distribution is unlimited. 


SnC QUALITY CTawBffl rp p 8 


19990406 064 





REPORT DOCUMENTATION PAGE 

Form Approved 

OMB No. 0704-0188 

Public reporting burden for this collection of information is estimated to average 1 hour per response, including the time for reviewing instruction, 
searching existing data sources, gathering and maintaining the data needed, and completing and reviewing the collection of information. Send 
comments regarding this burden estimate or any other aspect of this collection of information, including suggestions for reducing this burden, to 
Washington headquarters Services, Directorate for Information Operations and Reports, 1215 Jefferson Davis Highway, Suite 1204, Arlington, VA 
22202-4302, and to the Office of Management and Budget,.Paperwork Reduction Project (0704-0188) Washington DC 20503. 

1. AGENCY USE ONLY (Leave blank) 2. REPORT DATE 3. REPORT TYPE AND DATES COVERED 

March 1999 Master’s Thesis 

4. TITLE AND SUBTITLE 

DESIGN, IMPLEMENTATION, AND TESTING OF A REAL-TIME 
SOFTWARE SYSTEM FOR A QUATERNION-BASED ATTITUDE 
ESTIMATION FILTER 

5. FUNDING NUMBERS 

6. AUTHOR(S) 

Duman, Ildeniz 

7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) 

Naval Postgraduate School 

Monterey, CA 93943-5000 

8. PERFORMING 

ORGANIZATION REPORT 
NUMBER 

9. SPONSORING / MONITORING AGENCY NAME(S) AND ADDRESS(ES) 

10. SPONSORING/ 

MONITORING 

AGENCY REPORT NUMBER 

11. SUPPLEMENTARY NOTES 

The views expressed in this thesis are those of the author and do not reflect the official policy or position of 
the Department of Defense or the U.S. Government. 

12a. DISTRIBUTION / AVAILABILITY STATEMENT 

Approved for public release; distribution is unlimited. 

12b. DISTRIBUTION CODE 

13. ABSTRACT (maximum 200 words) 

Human hmb segment angle tracking requires a system which can track through all orientations. The 
major problem addressed by this research was to develop a real time inertial motion tracking system based on 
quaternions to overcome the singularities of Euler angle filters. 

This work involves clarification of the theory behind quaternion attitude estimation and development 
of a system capable of determining the orientation of an object in world coordinates. System sensors were 
built using miniature accelerometers, rate sensors, and magnetometers. The software system was designed by 
using Unified Modeling Language (UML) with object oriented design techniques. The actual 
implementation created a real time orientation tracking system. 

The system was tested with dynamic tilt table experiments. Test results showed that the quaternion 
attitude estimation filter system can track human limb segments in real time within 1 degree of accuracy in 
any orientation and with a 55 Hz. update rate. 

14. SUBJECT TERMS 

Quaternions, Euler Angles, Quaternion Attitude Estimation Filter, Inertial Motion Tracking 

15. NUMBER OF 
PAGES 

181 

16. PRICE CODE 

17. SECURITY CLASSIFICATION OF 
REPORT 

Unclassified 

18. SECURITY CLASSIFICATION OF 
THIS PAGE 

Unclassified 

19. SECURITY CLASSIFI-CATION 

OF ABSTRACT 

Unclassified 

20. LIMITATION 

OF ABSTRACT 

UL 


NSN 7540-01-280-5500 Standard Form 298 (Rev. 2-89) 


Prescribed by ANSI Std. 239-18 


1 





















11 



Approved for public release; distribution is unlimited 


DESIGN, IMPLEMENTATION, AND TESTING OF A REAL-TIME SOFTWARE 
SYSTEM FOR A QUATERNION-BASED ATTITUDE ESTIMATION FILTER 


Ildeniz Duman 

Lieutenant Junior Grade, Turkish Navy 
B.S.C.S., Turkish Naval Academy, 1992 


Submitted in partial fulfillment of the 
requirements for the degree of 


MASTER OF SCIENCE IN COMPUTER SCIENCE 


from the 


NAVAL POSTGRADUATE SCHOOL 
March 1999 



Approved by: 



Eric R. Bachmann, Thesis Co-Advisor 



IV 





ABSTRACT 


Human limb segment angle tracking requires a system which can track through all 
orientations. The major problem addressed by this research was to develop a real time 
inertial motion tracking system based on quaternions to overcome the singularities of 
Euler angle filters. 

This work involves clarification of the theory behind quaternion attitude 
estimation and development of a system capable of determining the orientation of an 
object in world coordinates. System sensors were built using miniature accelerometers, 
rate sensors, and magnetometers. The software system was designed by using Unified 
Modeling Language (UML) with object oriented design techniques. The actual 
implementation created a real time orientation tracking system. 

The system was tested with dynamic tilt table experiments. Test results showed 
that the quaternion attitude estimation filter system can track human limb segments in 
real time within 1 degree of accuracy in any orientation and with a 55 Hz. update rate. 
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I. INTRODUCTION 


A. MOTIVATION 

For many years, computer scientists have tried to create realistic virtual 
environments (VE) for simulation, training, evaluation, data visualization, computer 
aided design, tele-operation, tele-presence, robotics and entertainment. During this 
period, computer performance increased rapidly in both hardware and software 
[HENA97]. Many techniques and devices have been discovered and introduced to 
interface computers and humans. These advances are needed to deal with the well- 
known “human machine interaction problem” [HENA97]. Special interaction devices are 
needed to achieve the goal of total immersion of humans into virtual environments 
[HENA97]. The main thrust of research in this area has been directed toward producing 
new and improved sensors for tracking objects and humans. 

There are currently five fundamental tracking technologies; namely, mechanical, 
electromagnetic, acoustic, optical and inertial [DURL95]. Very recently, due to advances 
in component technology, inertial tracking systems have become more promising than 
other tracking technologies [SKOP96]. Inertial tracking systems use a combination of 
angular rate, accelerometer and magnetic sensors. Filter algorithms combine these sensor 
readings to obtain limb segment orientations. Such trackers are free of most of the 
problems of other tracking systems. 

B. PROBLEM STATEMENT 

Several methods exist for describing the orientation of an object. The most 
widely used method is to use Euler angles to describe attitude. However, tracking 
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systems using Euler angles are not capable of tracking objects in all orientations due to 
gimbal lock singularities [BACH96, SKOP96]. 

A quaternion based attitude estimation filter has been proposed to overcome the 
singularities and the divide by zero errors encountered when using Euler angles to 
represent orientations [MCGH96]. The theory behind the quaternion attitude estimation 
filter has been proved to be correct and a simulation program has been written in ANSI 
Common Lisp using only static sensor readings [HENA97]. The goal of this thesis is to 
develop a real-time quaternion-based orientation estimation filter in a PC environment, 
quantitatively test this system with a tilt table, and qualitatively test it when mounted on a 
human limb segment [USTA99]. Before the real-time software development, white noise 
effects on this filter are examined with a simulation program. In this thesis, the Unified 
Modeling Language (UML) [DOUG98] is used for software development, and the C++ 
programming language for system implementation. 

C. ORGANIZATION 

Chapter II of this thesis surveys existing tracking technology and related work in 
the area of inertial tracking systems. Chapter III discusses representations of rigid body 
orientation with Euler angles and quaternions, and presents a comparison between Euler 
angles and quaternions. Chapter IV presents the detailed mathematical theory behind the 
quaternion attitude estimation filter, white noise effects on the filter, and the derivation of 
the gain constant in the filtering process. Chapter V explains the hardware configuration 
of the system. Chapter VI presents the software design and development process. 
Chapter Vn presents the test results of this real time implementation of the filter with a 
tilt table. The last chapter. Chapter VIII, presents the conclusions of this research. It 
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introduces the possibility of using this system as an inertial tracking system in the NPS 
Autonomous Underwater Vehicle (AUV) project [YUN97] and the body suit project 
[ZYDA97]. Recommendations are made for future work and ways to improve the 
quaternion attitude estimation filter. 
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n. BACKGROUND 


A. INTRODUCTION 

A basic requirement in virtual environments (VE) is the tracking of objects, 
especially humans. Tracking of humans creates a convenient human machine interface to 
the virtual environment. If the user is to interact in a natural way with a virtual 
environment, then the use of standard 2D devices becomes unacceptable [FREY96]. 3D 
user interfaces in a virtual environment require the use of devices that are special to that 
environment. There are many 3D user interface systems available; however, they all 
have unique problems in real-time applications. This chapter presents current motion 
tracking devices, their usage in virtual environments, and their effectiveness when 
applied to real-time human motion tracking. 

B. REQUIREMENTS FOR SPATIAL TRACKERS 

Tracking devices aUow a virtual reality system to display the x, y and z position and 
the yaw, pitch and roll orientation of a tracked object or a human body part. The primary 
purpose of any tracking device is to provide an intuitive interface between human and 
machine [HENA97]. Human machine interfaces include all the devices used in a virtual 
environment system to present information to the human or to sense the actions or 
responses of the human [DURL953. Before selecting an appropriate tracking device, it is 
necessary to determine the characteristics and behaviors of the tracked objects. 

For normal arm movements during reaching, a fast motion is accomplished in 

about 0.5 seconds, wrist tangential velocities are about 3 m/s and the accelerations are 

about 5-6 g. For the fastest arm motion such as throwing a baseball, good pitchers 

release the ball at 37 m/s and accelerate their hands at more than 25 g. Motion 
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bandwidths of normal arm movements are around 2 Hz; the fastest hand motions are at 
around 5-6 Hz. The frequency content of normal arm motion can be defined as 5 Hz with 
a sampling rate of roughly 100 Hz [DURL95]. 

Head movements can be as fast as 1000 deg/s in yaw. Usual peak velocities are 
about 600 deg/s for yaw and 300 deg/s for pitch and roll. Tracker-to-host reporting rates 
must be at least 30 Hz. Delays of 60 ms or more between head motion and visual 
feedback are known to impair adaptation and the illusion of presence. Much smaller 
delays may cause simulator sickness [DURL95]. 

C. MOTION TRACKER PERFORMANCE 

Several different tracking devices and technologies have been developed and 
applied to virtual environment applications. [MEYE92, SKOP96, HENA97] suggest 
some key measures by which tracking systems may be evaluated, namely; 

• resolution and accuracy 

• responsiveness 

• robustness 

• registration 

• sociability 

Resolution can be defined as the smallest change, which can be detected by a 
given tracking system [HENA97]. Accuracy is the sensor error range. The precision 
with which actions can be executed in the virtual world depends on the resolution and 
accuracy of a tracking device used. The range of a tracking device is the maximum 
distance between the sensor and source up to which the position and the orientation can 
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be measured with a specified error [BARA93]. Accuracy would also include sensor drift, 
i.e., the tendency of output to change without any change in input [SKOP96]. 

Responsiveness is a measure of the quickness with which new information is 
provided. It is determined by sampling rate, data rate, update rate, and latency 
[SKOP96]. Sampling rate is simply how often the sensor is checked for new data. Data 
rate is defined as the number of computed data points per second that the system can 
provide. Many systems will implement a much higher sampling rate than data rate in 
order to assure that new data is not missed. Filtering the sensor data takes time and will 
hinder real time updates. The rate at which the system can provide updated position and 
orientation data to the host computer is the update rate. Latency is perhaps the most 
important characteristic of responsiveness. The usefulness of tracking devices in virtual 
environments depends to a large degree on whether the computer can track the 
movements of the user fast enough to keep the virtual world synchronized with the user’s 
actions. T his ability is determined by the lag of the signal, and the sensor’s update rate. 
The signal lag is the delay in time between a change of the position and orientation of the 
target being tracked and the report of the change to the computer. Lags above 50 ms are 
perceptible to the user and affect human performance. Typical update rates are between 
30 and 60 updates per second [BARA93]. A system’s lag is sometimes referred to as its 
latency and is one of the most important specifications of a tracking system [HENA97]. 

Robustness is a measure of the tracker’s effectiveness in the presence of noise or 
other signal interference in the operating environment [SKOP96]. Types of interference 
include physical, metallic, electronic and acoustic [HENA97]. Depending on the 
technology used, sensors may be sensitive to metal objects, radiation from display 
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monitors, and various noise sources or objects coming between source and sensor. This 
interference can limit the effectiveness of tracking devices [BARA93]. 

Registration is the correspondence between a unit’s actual position and 
orientation and its reported position and orientation over the domain of the working 
volume [SKOP96]. 

Finally, sociability describes a system’s maximum range of operation, its working 
volume, and the ability to track multiple targets within that operating range. The working 
volume is that volume in which the tracker can accurately report position and/or 
orientation information [MEYE92, HENA97]. In addition to considering these 
performance factors, one might consider availability, cost and ease of use before actually 
selecting a tracking system [SKOP96]. 

D. TYPES OF MOTION TRACKERS 

Most currently used tracking devices are active; that is, a generated source is 
attached to the object to be tracked or sensed by devices on the tracked object. In passive 
tracking, the target is monitored from a distance by one or several cameras [BARA93]. 
Current tracking devices are based on electromagnetic, acoustic, mechanical, optical and 
InfraRed (IR) technologies. 

1. Mechanical Tracking Devices 

Mechanical trackers measure changes in position and orientation by using jointed 
linkages directly connected to a point of reference. For body motion tracking, the point 
of reference can either be another part of the human body or a fixed surface near the 
human [SKOP96]. These trackers can be separated into two basic types, body based (ex- 
oskeletal, goniometer systems) and ground-based systems [DURL95]. Body based 
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systems are used to track the user’s joint angles or end-effector positions relative to some 
other part of the body. Ground-based systems are attached to some surface near the user. 
Generally, the user grasps an implement whose position and orientation are tracked 
[SKOP96]. 

Since no external source is required, these sensors are not susceptible to external 
interference [HENA97], and are much less sensitive to their immediate environment than, 
say, electromagnetic trackers [HAND93]. The lag for mechanical trackers is very short, 
less than 5 ms, their update rate is fairly high, 300 updates per second, and they are very 
accurate [BARA93]. The physical linkages are well suited for providing haptic 
responses. Haptic responses are force feedback cues that enable the user to experience 
simulated exertion forces during a virtual environment simulation. These cues further 
enhance the realism of the environment and the immersion of the user [HENA97]. 

The fact that mechanical trackers are a system of physical linkages attached to the 
body or constantly held makes them cumbersome. The main disadvantage of mechanical 
trackers is that the user’s motion is constrained by mechanical devices. These devices 
have a restrictive working volume and are usually not portable. They require a 
designated area for their use [HENA97]. 

Since they have moving parts, mechanical trackers can wear out after a period of 
time [HAND93]. Mechanical trackers tend to be accurate, responsive, robust and 
inexpensive, but they have poor sociability [MEYE92, SKOP96] and can be difficult and 
time consuming to calibrate [DURL95, PRAT94, SKOP96]. Applications requiring a 
limited range of motion and where user immobility is not a problem are best suited for 
this type of tracking system [MEYE92, HENA97]. 
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2. Electromagnetic Tracking Devices 

Electromagnetic trackers utilize artificially generated signals from the 
electromagnetic spectrum. In this case, the electromagnetic spectrum is defined in a 
narrow sense to mean radio and microwave frequencies [SKOP96]. 

An electromagnetic tracker comprises a transmitter and a receiver. A fluctuating 
magnetic field generated in the three orthogonal coils of a transmitter is picked up by 
three corresponding coils in the receiver. The variations in the received signal can be 
used to calculate the relative position and orientation of the receiver and transmitter. The 
fluctuating magnetic field may be Alternating Current or Direct Current [HAND93]. 

An alternative method recently proposed involves use of spread-spectrum ranging 
techniques [BIBL95, SKOP96]. This technique uses the measured time of flight of 
electromagnetic pulses to a receiver to determine range from a set of fixed transmitters. 
The concept is similar to that of the Global Positioning System (GPS). A minimum of 
three transmitters would be required to determine position via triangulation. A fourth 
transmitter would be required to ensure time can be accurately computed by the receiver. 
Transmitted signals would all occupy the same wide bandwidth and could utilize code 
division multiple access (CDMA) to preclude mutual interference [SKOP96]. 

Electromagnetic trackers have been commercially available for some time and are 
relatively inexpensive and easy to use [SKOP96]. These devices are generally very 
flexible due to the small size of the receiver (smaller than a 1” cube). Although the 
working volume is generally not very large, a few feet, it is usually possible to arrange 
various combinations of time-multiplexed transmitters and receivers to cover more space 
and track more objects [HAND93]. These systems tend to have good accuracy in a small 
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working space, with accuracy trailing off as distances from the transmitter increase 
[SKOP96]. 

Electromagnetic trackers suffer from several sources of error. Electromagnetic 
interference (EMI) from devices such as radios or display units can cause erroneous 
readings. Large objects made of ferrous metals can interfere with the electromagnetic 
field, again causing inaccuracies [HAND93]. 

Robustness is adversely effected by sensitivity to ferromagnetic objects in the 
vicinity, with alternating current based trackers being more suspectible than direct current 
based trackers. Alternating current systems tend to generate eddy currents in metallic 
objects, which then cause their own electromagnetic interference [SKOP96]. 

Adding power to the transmitter to increase the working volume can increase 
noise. Both AC and DC systems are adversely impacted by noise from power sources. 
Responsiveness is poor compared to other methods [SKOP96]. 

Latency is a serious problem with electromagnetic trackers. Up until recently the 
typical latency for an electromagnetic device was around 100 ms. This is mostly due to 
the filtering being performed on the data to remove noise. Newer versions of these 
devices have attempted to address this problem [HAND93]. Sociability is best in an 
environment without ferromagnetic occlusions, but is limited due to a small range of 
operation. Still, these systems can be very effective at shorter ranges [SKOP96]. 
Electromagnetic tracking works best in apphcations which require a limited working 
environment that is free of electromagnetic interference. 
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3. Acoustic Tracking Devices 

Acoustic devices are sometimes called “ultrasonic trackers”. Acoustic tracking is 
a fairly simple and well-understood technique [HAND93]. Such devices use high 
frequency ultrasonic sound waves for measuring the position and orientation of the target 
object by either phase-coherence tracking or time-of-flight (TOF) tracking [BARA93]. 

Phase coherence tracking works by measuring the difference in phase between 
sound waves emitted by a transmitter on the target and those emitted by a transmitter at a 
known reference point. Time-of-flight works by measuring the amount of time that it 
takes for sound emitted by transmitters on the target to reach sensors located at fixed 
positions in the environment. The transmitters emit sound at known times and only one 
transmitter is active at a time [BARA93]. One transmitter and three sensors are required 
for 3 DOF while three transmitters and three sensors are necessary for full 6 DOF 
tracking [HAND93]. 

Simple acoustic tracking devices can be constructed at low cost. They offer better 
range of operation than magnetic systems but can suffer severe effects from shadowing 
that can occur when tracked body parts are blocked by other objects [SKOP96]. Objects 
that move farther than half a wavelength in one update period will induce tracking error. 
The speed of sound in air varies with air temperature, pressure and humidity. Hence, 
calculations of distance may be incorrect due to environmental conditions unless steps are 
taken to account for these [HAND93]. Time-of-flight trackers typically suffer from a low 
update rate, brought about by the low speed of sound in air [BARA93]. Another 
common problem is that echoes of the sound signal will be reflected from acoustically 
“hard” surfaces, such as office walls, causing reception of “ghost” pulses at the sensor 
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and interference with other transmitted pulses [HAND93]. Time-of-flight tracking 
devices are vulnerable to spurious noise sources at any range [HENA97]. 

Phase-coherent systems enjoy many benefits over time-of-flight systems due to 
much higher data rates [MEYE92]. If the range is small, both systems offer good 
accuracy, responsiveness, and robustness. As range increases, data rates for time-of- 
flight systems decreases, causing responsiveness and robustness to decrease. Sociability 
of phase-coherent systems is better than that of time-of-flight systems due to larger 
working volumes [MEYE92]. 

4. Optical Tracking Devices 

There are a variety of approaches to optical sensing for position tracking and 
mapping. Distance may be measured by triangulation, by time-of-flight or by 
interferometry. The passive hght of the environment may be employed, structured light 
may be projected, light may be pulsed, or active or passive markers may be placed on a 
moving body. The different types of optical trackers can be broken into five categories; 
passive stereo vision systems, marker systems, structured light systems, laser radar 
systems, and laser interferometric systems [DURL95]. 

Passive stereo vision systems employ ambient hght and square-array charge- 
coupled device (CCD) cameras [DURL95]. Multiple images from cameras with varying 
viewpoints are compared. Triangulation is then used to determine position [SKOP96]. 
An essential issue is to solve the correspondence problem of relating the same points in 
two different images. Passive stereo vision systems are unlikely to be useful in virtual 
environment in the near term, as robustness and accuracy are not yet comparable to active 
ranging systems [DURL95]. 
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The stereo correspondence problem is solved in marker systems because a few, 
easily identifiable fiducial points are tracked on a moving body. To create very bright 
spots on the image a number of infrared light emitting diodes (IRED) are used. For 
detection, Icm^ position sensing detectors (PSD), also called lateral effect photodiodes, 
are used. Multiple markers can be tracked to yield orientation and to follow multiple 
bodies simultaneously. Workspace volume is about Im^. A fundamental problem with 
the use of PSD is reflection of IRED light from environmental surfaces [DURL95]. 

Structured light systems use a ray, plane of light, or a laser, to sweep across a 
scene. At each position of the plane, a light stripe is created, which is sensed by a two 
dimensional camera. The intersection of the known plane and the line of sight from the 
camera determines the three-dimensional coordinates. Another common method uses 
laser spot scanning of the scene. In this method either all or only portions of the scene 
may be scanned for data [DURL95]. 

Two of the most prevalent techniques that use lasers include laser radar and laser 
interferometry techniques. Laser radar works in the same way as acoustic ranging 
techniques, except that much higher data rates are possible. Techniques include both 
time-of-flight and phase shift. By scanning the entire scene, a three dimensional picture 
of the scene can be generated. Laser radar techniques are more appropriate for longer 
distances than laser techniques that use triangulation [DURL95]. 

Laser interferometry, uses a steered laser beam to track a reflector on the object 
being tracked. Phase-shift ranging and angular information from the steered system are 
used to determine position. An alternate method uses several lasers to track the reflector 
from different fixed positions to obtain range information. In this case, the intersection of 
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spheres whose radii are determined jfrom the range information determines the location of 
the point being tracked. The problem with these techniques is that they provide only 
incremental displacement data and loss of signal via shadowing can be cause for re¬ 
calibration [DURL95]. 

5. Inertial Tracking Devices 

Inertial tracking systems use a combination of linear acceleration, angular rate and 
magnetic sensors to determine rigid body orientation. Angular orientation is determined 
by integrating the output from the angular rate sensors. Angular rate sensors operate by 
using the differential combination of the outputs of two vibrating linear accelerometers. 
Angular rate sensor output has an error called drift. Drift is defined as the tendency of 
bias errors, inherent to the sensor, to cause increasing orientation estimation errors over 
time. This fundamental limitation makes angular rate sensors only a relatively short-term 
solution to determining a rigid body’s spatial orientation [HENA97]. 

In order to compensate for the long-term errors introduced by the use of angular 
rate sensors, inertial systems utilize linear acceleration sensors called accelerometers. 
Accelerometers measure the gravity vector, relative to the object being tracked as well as 
the forced linear accelerations of the attached rigid body. 

The third component of inertial systems is the magnetometer. The magnetometer 
is sensitive to the Earth’s magnetic field and can sense rotations about the local vertical 
axis. Magnetometers must be used to correct drift errors in the horizontal plane 
[HENA97]. 

Originally these devices were used in guidance systems for airplanes and missiles 
and as such were cumbersome [HAND93]. Several companies have begun 
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manufacturing small devices, but a complete system is not commercially available yet for 
body tracking. These systems hold much promise for future application to the body 
tracking problem. Larger systems have shown that accuracy, resolution, response, 
robustness and registration requirements for human body tracking can be met by this 
technology. Although the technology is currently expensive, it is expected that costs will 
come down as devices are marketed. The greatest benefit of this type of system is its 
sociability. Whereas electromagnetic, acoustic and optic devices all require emissions 
from a source to track objects, inertial tracking systems are sourceless. This precludes 
the inevitable disastrous effects of occlusions and noise [SKOP96]. Pure inertial systems 
are capable of tracking only orientation. A hybrid system providing 3D location of one 
reference point on the body would be required to fully solve the human body tracking 
problem. 

E. RELATED WORK 

Various methods exist for tracking the rotations of an object and representing 
these rotations in a virtual environment. Orientation filters based on Euler Angles are not 
capable of tracking orientations through the vertical, due to the gimbal lock singularities 
and divide by zero errors in equations involving trigonometric functions [BACH97B]. 

A specific example of an orientation filter based on Euler Angles is the navigation 
filter used in the NPS AUV project [MCGH95, BACH96]. The filter incorporates inputs 
from an onboard Inertial Measuring Unit (IMU), a compass, and a water-speed sensor. 
Intermittent GPS fixes periodically provide accurate real-time navigational data 
[BACH96, HENA97]. A schematic representation of the attitude estimation part of the 
SANS navigation filter is shown in Figure 1. 
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Figure 1: Euler Angle Attitude Filter [HENA97] 

The SANS filter experiences singularities in two separate places. The 

accelerometer estimate of the roll angle and the angular rates of roll and azimuth become 

undefined at elevation angles of ±90 degrees. 

In past research, [SKOP96] Euler Angles were used to track and to represent the 

human upper body. SKOP96 was forced to employ error-checking programming 

techniques in the software to avoid the singularities of an Euler Angle based orientation 

filter. He concluded that current electromagnetic trackers lack sufficient accuracy and 

registration to enable their use as a true six degree of freedom (DOF) tracker in human 

body tracking applications and called for the investigation of new tracking technologies 

to support the insertion of dismounted infantry into a virtual environment [ZYDA97]. 

In order to avoid singularities in human body tracking or in a navigation system, 

an alternative representation is needed for orientation filters. The quaternion attitude 

estimation filter was proposed by [MCGH96] as an alternative representation and 

improvement to the filters based on Euler Angles. 
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HENA97 developed the software necessary to support simulation of a quaternion 
filter using inertial sensors. His software was written in ANSI Common Lisp. The filter 
was tested with a computer simulated inertial tracker and used only static sensor readings. 
This research showed that the theory behind the quaternion attitude estimation filter was 
sound. Reduced computational complexity was achieved since the quaternion filter uses 
no trigonometric functions. 

F. SUMMARY 

Excepting inertial and spread-spectrum systems, the systems described in this 
chapter are currently available [SKOP96]. Mechanical techniques for tracking the upper 
body have been implemented and have been shown to be cumbersome and difficult to 
use. Acoustic trackers can provide potentially excellent accuracy and resolution together 
with a greater range of operation than magnetic trackers. Hybrid spread-spectrum 
ranging and inertial tracking systems have potential for providing increases in accuracy, 
response and range of operation over systems available now [SKOP96]. 

This chapter presented a brief overview of current tracking technologies and past 
research into body tracking systems and orientation filters. More detailed information 
about tracking systems can be found in [DURL95]. The next chapter presents 
representations of rigid body motion with Euler angles and quaternions, and a 
comparison between Euler angles and quaternions. 
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in. RIGID BODY ROTATION 


A. INTRODUCTION 

Computer animation and robotics both involve manipulating, rotating and 
translating moving objects in a 3D environment. Several different systems are used to 
describe positions and motions in space. These include Euler angles and quaternions. 
Each of these has particular advantages and disadvantages. This chapter will provide the 
basic information about Euler Angles and Quaternions. 

B. EULER METHOD 

The most popular method for describing orientation in Earth coordinates is the 
Euler Method. Using a sequence of three angles, the Euler Method provides an intuitive 
description of attitude. Although eleven other possibilities exist, these angles typically 
consist of the familiar azimuth angle, \|/, the elevation angle, 8, and the roll angle, <p, 
[CRAI89]. Euler angles specify three successive rotations to bring the Earth coordinates 
into alignment with the body coordinates [COOK92]. 

1. Rigid Body Rotation with Euler Angles 

Euler’s theorem states that any numbers of rotations of a rigid body about an 
Earth-fixed axis are equivalent to a single rotation about a single Earth-fixed axis. If all 
rotations are about the north(x), east(y) and down(z) axes, as depicted in Figure 2, the 
angles are called Euler Angles. 
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Figure 2: Right Handed Earth Coordinate Axis 

As stated previously, there are twelve sets of rotations. The standard “azimuth. 


elevation, and roll” set is represented by the three rotation matrices given in Eq. 3.1. Rx 
represents a rotation about the x axis [CRAI89]. Similarly for Rz and Ry. 

R = R 2 RyRx (Eq. 3.1) 


These names above and the symbols \(/,0, and tp are reserved for the Euler angle 
set. The sign of a rotation is determined using the right hand rule. The ranges for the 
rotation angles are 


y/ = ±7!: 9 = ±— (p = ±7r 

The x-axis rotation matrix (roll) is given by [CRAI89] 


Rx(tp) = 


1 0 0 

0 costp - sintp 

0 sintp coscp 


The y-axis rotation matrix (elevation, pitch) is given by 

Ry(0) = 


cos 9 

0 

sin0 

0 

1 

0 

-sin^ 

0 

cos 9 


(Eq. 3.2) 


(Eq. 3.3) 


(Eq. 3.4) 
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The z-axis rotation matrix (azimuth, heading, yaw) is given by 


Rz(V^) = 


cos y/ - sin 0 

sin^ cosy/ 0 

0 0 1 


(Eq. 3.5) 


In general, every rotation matrix is a 3x3 matrix, but to make both translation and 
rotation calculations using matrices, similar 4x4 homogeneous matrices are used. A 
homogeneous rotation matrix in graphics, takes the form [FOLE97] 


i? = 


r r r 0 
r r r 0 
r r r 0 
0 0 0 1 


(Eq. 3.6) 


Any rotation matrix can be put into this general form. Rotations of an object are 
made by multiplying these matrices with the homogenous coordinates of the object. In 
composing the results of successive rotations by matrix multiplication, the first rotation is 
associated with the right-most matrix. Depending on the order of the rotations, there 
exists more than one way to describe a given orientation. 

Any rotation can be calculated by multiplying these individual matrices in the 
right order. For example a roll rotation, followed by an elevation and followed by a 
change in azimuth would be calculated by multiplying the matrices from right to left 

R^R^RyR^ (Eq.3.7) 

resulting in 


R = 


cosy/cosO 

s\ny/cos6 

-sin^ 


sin ^in Ocos y/ - cos (psmy/ sin ^in y /+cos (pcos y/smd 

sin ^in ftin y /+cos ^os y/ cos ^ sin ^sin - cos ^ sin ^ 
cosfisin^ cos(pcos0 


(Eq.3.8) 
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Figure 3: Body Coordinate Axes 
2. Body Rates to Euler Rates 

To rotate a body, its Rotational Velocity around the nose vector (n), the side 
vector (s) and the approach or belly vector (a) can be calculated, Figure 3. This rotational 
velocity can be presented in Earth coordinates as 

^co ~ ®x ®y ®z] 3.9) 

and in body coordinates 

Bco=[pqrr {Eq.3.10) 

where p is roll rate, q is pitch rate and r is yaw rate. These words and symbols are 
reserved for body rotations [MCGH93]. Euler rates are in Earth coordinates and p, q, and 
r are in body coordinates. Thus, the following must be noted. 

• Roll rate, p roll Euler angle rate, (j) 

• Pitch rate, q ^ elevation Euler angle rate, 6 

• Yaw rate, r 9 ^ azimuth Euler angle rate, \j/ 
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The rotation rate in Earth coordinates is; 


'o' 


o' 


4> 

0 

+ Rz 

e 

+ RzRy 

0 



0 


_0_ 


Evidently,- the rotation in Body coordinates is; 


^co ~ 




‘CO 


(Eq. 3.11) 


(Eq. 3.12) 


Since 


= = R-'Ry-\-'E^ 



o' 


o' 


9 

R/Ry^R/ 

0 

y. 


e 

0 

*r/ 

0 

0_ 


= 


cos^ 

sin^in^ 

cos^in^ 


0 

cos 9 ? 
-sin 9 ) 


-sin0 

sin^os^ 

cos^os^ 


it follows that 


Likewise, 


and 




'o' 


0 

e 


cos^ 

o_ 


— sin^ 


«/«/«/ 


0 

0 

w 




-sin^ 

sin^os^ 

cos^os^ 




4> 


'f 

0 

= 9 

0 

0 


0 


(Eq. 3.13) 


(Eq. 3.14) 


(Eq. 3.15) 


(Eq. 3.16) 


(Eq. 3.17) 


(Eq. 3.18) 
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Thus 



-sin0 


0 


r 

^co ~ V 

sin q) cos 0 

+ 0 

costp 

+ (p 

0 


cos (p cos 0 


-sintp 


0 


= {pqry 


and 



-\j/sin0 + 9 


P 

^CO “ 

\j/sin (p cos 0 + 0 cos (p 

= 

q 


\j/cos cp cos 0 - 0 sin (p 


r 


In component form, this is 

p = ^-\j/s\nd 
q = \j/5in(pco^0 + 9c<35(p 
r = \j/ co5(pco50 — 0 sin cp 

Solving these equations in terms of p, q and r, results in 

6 = qcos(p-rsmq) 

\j/ = rstc$cos(p + qstcds,m(p 
^ — p + ridindco%q> + qX 2 in 9 sm(p 

Equivalently, in matrix form; 




e 

= 

y. 



1 tan ft in ^ tanftos^ 

0 cosq) -smcp 

0 secftin^ secftos^ 


(Eq.3.19) 


(Eq. 3.20) 


(Eq. 3.21) 
(Eq. 3.22) 
(Eq. 3.23) 

(Eq. 3.24) 
(Eq. 3.25) 
(Eq. 3.26) 


(Eq. 3.27) 


The equations jfrom Eq. 3.21 through Eq. 3.27, also known as the gimbal rate 
equations and are quite commonly used in animation and simulation [COOK92]. The 
matrix in Eq. 3.27 is also referred to as the T matrix [FRAN69]. 
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3. 


Problems with Euler Angles 


The first problem results in divide-by-zero errors in equations involving 
trigonometric functions. Whenever cos0 = 0 (0 = ±7t/2) 


^ sin0 ^ 1 

tan0 =-r , sec0 = — 


COS0 


COS0 


(Eq. 3.28) 


are undefined. 


A second problem exists when pitch, 0, goes through the vertical; 




(Eq. 3.29) 


At that point, the roll and azimuth axes become coincidental. This is called gimbal lock 
singularity. “Gimbal lock” is a term derived from a mechanical problem that arises in the 
gimbal mechanism used to support a compass or gyroscope. The final rotation matrix 
depends on the order of multiplication. It is sometimes the case that the rotation in one 
axis will be mapped onto another rotation axis. Even worse, it may become impossible to 
rotate an object in a desired axis [WATT98]. 

Both these problems occur because Euler angles ignore the interaction of the 
rotations about separate axes. In truth, these rotations are not independent of each other 
[WATT98]. 

The only solution to going through a vertical orientation is to fake; i.e., fix the 
code so a division-by-zero error doesn’t occur. These fixes usually prove less than 
satisfactory and make it impossible to track an object through all possible orientations 
[COOK92]. 
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C. QUATERNION METHOD 

An alternate method of describing orientation that has been gaining popularity in 
the graphics conununity is the use of unit quaternions. It is not a new method 
[COOK92]; quaternions have been around for more than 150 years. 

The quaternion is a concept related to three dimensional vectors, but which allows 
the representation of operations such as rotations not directly representable with vectors 
[PERV82]. A quaternion is a four dimensional vector with an associated quaternion 
product. It is conventional to interpret a quaternion as a generalization of a complex 
number with a real number part and a vector part. 

1. Quaternion Basics 

The basis for quaternions are three imaginary “flags” i, j and k where [MCCA90] 


=k^ = ijk = -1 

(Eq. 3.30) 

ij = -ji = k 

(Eq. 3.31) 

jk = -kj = i 

(Eq. 3.32) 

ki = —ik = j 

(Eq. 3.33) 


There are several equivalent ways of writing quaternions in terms of their four 
components; one way is the Standard Quadrinomial Form [PERV82]: 

Q = [a+fii + '}j + Sk:a,p,y,S real} (Eq. 3.34) 

There are three other commonly used quaternion notations. 

Linear combination of four components: 

q = w + xi + yi + 2 jc (Eq. 3.35) 
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Four dimensional vector: 


q = {wxy z) (Eq. 3.36) 

Scalar with a vector “imaginary part”: 

q = (w,v) (Eq. 3.37) 

where w is a scalar quantity and v is a vector. Quaternion addition is defined in 
the same manner as normal vector addition 

Q\ +^2)(>'l +y2)(^l ■•■ 22 )) (Eq. 3.38) 

If s is a scalar, then scalar multiplication with a quaternion is defined as 

sq-{sw,sv) (Eq. 3.39) 

Quaternion multiplication can be defined using Eq. 3.30 through Eq. 3.33. Let 

= Wj +ixi +jyi +kzj and q 2 = W 2 +ix 2 + jy 2 +kz2- Then 


q\ *q2 =(^ 1^2 -xiX2-y\y2 -ziZ2) + 

i{xiW2 + W 1 X 2 -z\y2 + yiZ2) + 
Ky\W2 +Z\X2 +yviy2 -XiZ2) + 
k{z\W2 - y\X2 + Xiy 2 + W 1 Z 2 ) 


(Eq. 3.40) 


Note that COOK92 has sign errors in quaternion multiphcation. The same result can be 
accomplished through a vector dot product, vector cross product and vector scalar 
multiphcations [MCGH98A]. 

q\*q 2 = (^ 1^2 -Vi •V 2 ,'W\V 2 +>^ 2^1 +^1 XV 2 ) (Eq. 3.41) 


Because of the vector cross product, the quaternion product is not commutative. Some 
mathematical facts related to quaternions are as follows [SHOE94, CRAI89] 

Quaternion addition rules; 

^1+^2 =52+^1 (Eq.3.42) 


iq \ +^2)+^3 - q \ +(^2 +^3) 
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(Eq. 3.43) 



^ + 0 = 0 + ^ 


(Eq. 3.44) 


q + {-q) = {-q)+q = ^ 

(Eq. 3.45) 

Quaternion multiplication rules; 


iq\q2)q^ = q\iqiq^) 

(Eq. 3.46) 

11 

1! 

(Eq. 3.47) 

qq ^ = q ^q = \ 

(Eq. 3.48) 

if q^qi = 0, then either = 0 or ^2 = 0 

(Eq. 3.49) 

(^1 +^ 2)^3 =^1^3+^293 

(Eq. 3.50) 

The quaternion conjugate; 


q* = (w,-v) = {w, -xi -yj -zk)-{w -x -y -z) 

(Eq. 3.51) 

The quaternion Norm; 


N(q) = qq* = + |vp = + v • v = |q|^ 

(Eq. 3.52) 

The magnitude of a quaternion is the square root of its norm. 


M(q) = y[N(^ 

(Eq. 3.53) 

The quaternion inverse is evidently 


* 

q-^- ^ 

^ N{q) 

(Eq. 3.54) 

and for a unit quaternion 


-1 * 
q =q 

(Eq. 3.55) 


which is easier to compute than the inverse of a matrix. 
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The normalized unit quaternion is given by 


(1 q q. 

‘I^nmlized ^ ^ 

Any scalar can be represented as a quaternion. 

^ = ( 5000 ) = ( 5 , 0 ) 

Any three dimensional vector can be represented as a quaternion. 

^ = (0j:yz) = (0,v) 

As an alternative, a quaternion can be represented as 

^ = w +V 

The vector part of quaternion can be rewritten as 

V = sr = rs 

where. 


5 = |v| = ■y/(x^ +y^ +Z^) 


and r is the unit vector given by 



V 



(Eq. 3.56) 


(Eq. 3.57) 

(Eq. 3.58) 

(Eq. 3.59) 

(Eq. 3.60) 

(Eq. 3.61) 


(Eq. 3.62) 


With this notation a quaternion becomes 


q = w + rs 

Another representation, the quaternion exponential form 

a ^w+rj w rs 
=e = e e 

e’^® = coss+r sins 


(Eq. 3.63) 

is defined as [MCGH98A]; 

(Eq. 3.64) 
(Eq. 3.65) 
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The conjugate becomes 


* ^ 
q -w-rs 


(Eq. 3.66) 


2. Rigid Body Rotation with Quaternions 

The orientation of a rigid body can be described as a rotation {d) about a single 
inclined axis (u). Constraining the axis (m) to be of unit magnitude, the unit quaternion 
iq) representing this rotation is [MCGH97] 


fee' 

q= Cos—.uSin — 
^ V 2 2. 


(Eq. 3.67) 


where m is a unit vector describing the axis about which the vector p is to be rotated 
through an angle e [BACH97B]. The rotation of a vector, p by a quaternion, q is defined 
as [SHOE85, FUND96] 

Protated = (Eq. 3.68) 

The quaternion rotation defined in Eq. 3.68, rotates the vector’s perpendicular component 
twice the angle which is perpendicular to the rotation axis, and leaves the vector’s 
parallel component unchanged. To obtain the desired rotation half of the rotation angle is 
used in the construction of the unit quaternion given by Eq. 3.67 [MCGH98A]. 

Every rotation has two representations in quaternion space, namely q and -q, with 
the same effect [WATT98]. By using this topological oddity, Eq. 3.68 can be rewritten 
as 

Protated = (Eq. 3.69) 

The product of two unit quaternions is always of unit magnitude. The product 

q 2 qi produces a single quaternion describing an orientation achieved by applying q 2 

relative to the orientation described by where both quaternions are expressed in Earth 
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coordinates. If rotations are described by quaternions in body coordinates, then the order 
of the product is reversed. 

It should be noted that, unlike Euler angles, quaternion rotations require only two 
trigonometric functions to rotate a vector and experience no singularities at any angle of 
rotation. 

To illustrate quaternion rotation through an example, let p=(0 10 0) describe a 
body oriented along the x-axis, wings level, headed to north. A positive 90° rotation 
about the y-axis can be represented by 

q = (Co 545° , v5m45° ) (Eq. 3.70) 

= (0.707 0 0.707 0) (Eq. 3.71) 

where v = (0 1 0). 

The following product rotates the body with an orientation described by p 90° 
about the y-axis 

Protated = (Eq* 3-72) 

= (0 0 0-1) (Eq.3.73) 

which represents an orientation along the -z-axis, in which the body is pointing straight 
up [BACH97B]. 

3. Derivation of Quaternion Rates from Body Rates 

Angular rates p, q and r, can be used to find the derivative of the orientation, q , 
relative to an Earth fixed coordinate system. 

For small 0 




e 

2 


(Eq. 3.74) 
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Thus 


( ^ ■ 


( e'] 

cos—,v sin 


1,V — 

V 2 

27 

L 27 


(Eq. 3.75) 

Assuming 9 changes linearly with time, the orientation expressed by ^ as a 
function of time becomes, for small t: 




(Eq. 3.76) 


The vector vd, expresses an angular rate of 6 about a vector v in body coordinates. Thus 

v0 = (p ^ r) (Eq. 3.77) 

and q(t) becomes 


,111 


Taking the derivative of q(t) with respect to time produces 


^ ^ • ml 1 In 

-,(0 = « = (0.-p-,-r) 


= -{0pqr) 


(Eq. 3.78) 




^a 


(Eq. 3.79) 

(Eq. 3.80) 
(Eq. 3.81) 


If is the initial orientation in Earth coordinates and q 2 is a second rotation in 
body coordinates then qs is the composite of the two rotations 

qz=q\q2 (Eq. 3.82) 
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By the product rule 


1 

^3 = mi +m 2 = ^ 1^2 = 2 ^\Bco (Eq. 3.83) 


In general [BACH97B] 

Q = ^^B(ii=^QiOpqr) (Eq. 3.84) 

This general formula can be used with Euler integration to achieve a smooth 
rotation. Eq. 3.84, also avoids the “branch cut” problem of the Euler angles. After every 
full 360 degree rotation, the quaternion representing the rotation will switch to its 
negative [MCGH98A] as presented in Eq. 3.69. 

4. Concatenating Rotations 

Suppose qi and q 2 are unit quaternions representing two rotations relative to Earth 
coordinates, and it is desired to perform qi first and then q 2 . To do this, we apply q 2 to 
the result of qi, regroup the product using associativity, and find that the composite 
rotation is represented by the quaternion q 2 * qi. 

q2 *(9l *P*qr^)*q2“^ =(q2 *qi)*P*(qf^ *92’^) (Eq- 3.85) 

= (q 2 *qi)*P*(q 2 *qi)~^ (Eq. 3.86) 

5 . Computing Rotation Matrices from Quaternions 

The only time we need to compute a matrix is when we want to transform the 
object using a homogenous transformation. Alternatively, rotation and translation can be 
handled separately, eliminating the need for computing the rotation matrix [SKOP96]. 
Matrix multiplication requires many more operations than a quaternion product. Thus, 
efficiency and numerical accuracy is improved through the use of quaternions rather than 


33 


matrices. A rotation matrix for the given rotation can be calculated from a quaternion 
representing that same rotation. The general rotation matrix is as follows; 


Q, 


matrix 


2xy-2wz 
2xz+ 2wy 
0 


2xy+2wz 


2xz - 2wy 


7 7 0 0 

w^-x^+y"^-z 2x)’ + 2wx 


2yz-2wx 

0 


2 2 2^2 
w -X -y +z 


0 
0 
0 

2 2 2 2 
+x‘^ +y +z 


(Eq. 3.87) 


The simplified version of this matrix for unit quaternions is [SHOE85]; 


a 


matrix 


\-ly^-2z^ 

2xy - 2wz 

2xz + 2wy 
0 


27cy + 2wz 2xz - 2wy 

1 - 2x^ - 2z^ 2xy + 2wx 
2yz - 2wx 1 - 2x^ - 2y^ 


0 


0 


0 

0 

0 

1 


(Eq. 3.88) 


Using this matrix, the rotations of a vector P can be computed as [COOK92] 

Protated = P*Qmatrix (Eq. 3.89) 


6. Slerp 

The use of linear interpolation between two unit quaternions produces non-unit 
quaternions without normalization. Spherical linear interpolation {slerp) between two 
unit quaternions is a natural generalization of linear interpolation [SHOE85] and is 
obtained by 


sin(l - m)Q 

slerpiq-^ ,q2,u) = q^ -—— + ?2 

sinLz 


sinirQ 
sin £2 


(Eq. 3.90) 


qi • q 2 = cosQ (Eq. 3.91) 

where u is between 0 and 1, and Q is the interpolation angle between two key 
quaternions. For the opposite direction, the interpolation angle becomes 2ti - Q. 
[WATT98]. 
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Given any two key quaternions, p and q, there exists two possible arcs along 
which we can move. One of these arcs is shorter. A method for finding the shortest 
interpolation between the quaternion pairs p and q, and p and -q is as follows. First find 
the magnitude of their difference, that is {p-q)*{p-q), and then compare this to the 
magnitude of the difference when the second quaternion is negated, that is 
{p+q)*{p + q). If the former is smaller, then we are already moving along the smaller 
arc and nothing needs to be done. If, however the second is smaller, then we replace q by 
-q and proceed [WATT98]. 

D. COMPARISON OF EULER ANGLES AND QUATERNIONS 

Quaternions and Euler angles each have their own advantages and disadvantages. 
The most significant advantage of quaternions is that no singularity exists when the 
elevation angle (0) passes through ±7t/2. Any orientation or rotation can be represented 
by quaternions. In the Euler Method, roll and azimuth Euler angle rates become 
undefined due to division by zero. Truncating the angles at ±3i/2 will avoid this problem. 
However, this truncating will result in some skipping during the rotation [COOK92]. 

Quaternion rotation avoids the “branch cut” problem of Euler angles. When using 
quaternions to rotate an object, after every full rotation the quaternion representing the 
rotation will switch to its negative. 

Quaternions can be computed directly from the dynamic equations, bypassing the 
computation of transcendental functions necessary in computing Euler angles. This 
direct arithmetic operation reduces the cost of computation compared to matrix 
multiplication. 
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Quaternions are compact and simple. However, there are three difficulties with 
using quaternions. First, each orientation of an object can actually be represented by two 
quaternions. Since rotation about the axis, v by an angle 0 is the same as rotation about 
-V by the angle -6; the corresponding quaternions are antipodal points on the sphere in 
4D. Second, orientations and rotations are not exactly the same thing. In an animation, 
360° rotation is very different from a rotation of 0°. With the first one, the model will be 
animated for a full rotation. In the second one, no animation will be executed. After the 
rotation however, the same quaternion (1 0 0 0) or (-1 0 0 0) represents both. Thus 
specifying multiple rotations with quaternions requires intermediate control points 
[FOLE97]. 

The third difficulty is that quaternions provide an isotropic method for rotation. 
The interpolation is independent of everything except the relation between the initial and 
final rotations. This is useful for interpolating orientations of tumbling bodies, but not for 
interpolating the orientations of a virtual camera in a scene. Humans strongly prefer 
cameras to be upright, and are profoundly disturbed by tilted cameras [FOLE97]. By its 
very nature, the notion of a preferred direction cannot easily be built into the quaternion 
representation [WATT98]. Quaternions have no such preferences, and therefore should 
not be used for camera interpolation. The lack of an adequate method for interpolating 
complex camera motion has led to many computer animations having static cameras or 
very limited camera motion [FOLE97]. 

E. SUMMARY 

This chapter presented a brief overview of two methods, namely Euler and 
Quaternion methods, used to represent the rotations and orientations of a rigid body. This 
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chapter also provided some introductory information about quaternion operations. The 
next chapter introduces the theory and the mathematical formulation of a quaternion 
attitude estimation filter. 
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IV. QUATERNION ATTITUDE ESTIMATION FILTER 


A. INTRODUCTION 

The purpose of any filter is to separate one thing from another [BROW97]. The 
main purpose of the Quaternion Attitude Estimation Filter is to combine independent 
noisy inertial measurements to determine the orientation of a tracked object. The 
Quaternion Attitude Estimation Filter uses different types of instruments to get the 
measurement values and implements a nonlinear complimentary filter. Orientation 
estimates are corrected by minimizing the mean square measurement error. The 
Quaternion Attitude Estimation Filter is designed as a complimentary filter. However, 
with the addition of bias estimation, it can no longer be considered a complimentary 
filter. This chapter will present the mathematical theory of the quaternion filter, the filter 
linearization theory, bias estimation considerations and the white noise effects on the 
filter. 

B. QUATERNION ATTITUDE ESTIMATION FILTER 

The Quaternion Attitude Estimation Filter was proposed by [MCGH96] as an 
alternative representation and improvement to filters based on Euler Angles. The 
Quaternion Attitude Estimation Filter is designed to track human limb segments through 
all orientations as part of an inertial tracking system. It uses three different types of 
sensors to obtain the information about the orientation of a tracked object. These sensors 
are a three-axis accelerometer, a three-axis angular rate sensor and a three-axis 
magnetometer. These sensor inputs appear in Figure 4 as (xyz),(pqr), and 
{bi b 2 by, respectively. 
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Magnetometer 



Figure 4: Quaternion Attitude Estimation Filter 
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Angular Rate Sensor 





The three-axis accelerometer measures the combination of forced linear 
accelerations and the reaction force due to gravity, a^^sured = ^ - g [FOXL94]. 

The three-axis angular rate sensors measure angular velocity. The angular rate 
sensor experiences drift errors over time. To correct the measured angular rate sensor 
data in both the horizontal and vertical planes, accelerometer and magnetometer 
information is required. 

Normally, the direction of the Earth’s gravity vector, m, expressed as a unit 
vector, is down. For low fi’equency considerations, the linear accelerations of a body 
average to zero. That is, in the long term, the accelerometer senses only those 
accelerations due to gravity [BACH97B]. Thus, on the average, the three-axis 
accelerometer returns the gravity vector or the local vertical, aj^ig^sured = ~S [HENA97]. 
The down unit vector in quaternion form can be represented as 

m = [0001] (Eq.4.1) 

The three-axis magnetometer measures the Earth’s magnetic field, b, in body 
coordinates [HENA97]. The main purpose of the magnetometer is to sense the drift error 
of the angular rate sensor about the vertical axis. This can not be sensed by the 
accelerometer [HENA97]. The Earth’s unit magnetic field, n, at any location is known 
and can be calculated or looked up [BACH97B]. The Earth’s unit magnetic field can be 
represented as a unit vector by; 

n = [0nin2n3] (Eq. 4.2) 

The characteristics of the Earth's magnetic field in the local area must be 
determined to find n, which incorporates the declination and the dip angle. Magnetic 
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declination is defined as the differrence between the north compass heading and the true 
geographic north at a given location on the Earth's surface. Monterey, CA requires a 
correction of 15°. The lines of the Earth's magnetic field are parallel to the surface at the 
equator. However, as one approaches the magnetic poles, they become increasingly 
vertical. Dip angle is the correction of the measure for the downward deflection of the 
local magnetic field. In Monterey, a correction of -60° is applied [HENA97]. 

C. DERIVATION OF THE FILTER 

The Quaternion Attitude Estimation Filter takes normalized measurements from 
the three-axis accelerometer and from the three-axis magnetometer as shown in Figure 5 
[HENA97]. 

Magnetometer 

(i)l *2 63) 

J 


Figure 5: Upper Left Part of Quaternion Attitude Estimation Filter 

The accelerometer returns the local vertical, h and the magnetometer returns the 

Earth’s local magnetic field, b. This information is expressed in body coordinates as 
3-dimensional unit vectors in the form of the pure imaginary unit quaternions given in 
Eq. 4.3 and Eq. 4.4. 

h = [0hih2h^] (Eq.4.3) 



Accelerometer _^ 

{xyz) = {h^h^h^) 
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b = [Obi b2 b^ ] 


(Eq. 4.4) 


The filter combines these vectors to get the measured vector, jq given by; 


h 

h2 


yo = 


% 

h 

h 


h 


6x1 


(Eq. 4.5) 


After forming the measured vector, the filter calculates the computed 
measurement vector, y(^), using the estimated orientation quaternion, 4- The Earth’s 
unit gravity vector, m, and the local magnetic field vector, n, are rotated into body 
coordinates in order to permit comparison with the measured orientation vector. The 

^ A. 

estimated orientation of body, q, determines h and b by; 


h = q ^mq (Eq. 4.6) 

and 

b = q~\q (Eq. 4.7) 

Where the real or scalar part, w, is always zero. The estimated orientation 
quaternion is set when the system is initialized. The computed measurement vector is 
given by; 

. A. 1 A A 1 A. 

y{q) = {q niq,q nq) 


(Eq. 4.8) 




By removing the w values of h and b , the computed measurement vector can be 


constructed as; 







(Eq. 4.9) 


The error is computed by taking the difference between the measured vector and 
the computed vector and is given by 

= y(^)6x\ - J06;cl 


or 


hi-h 
h2 -h2 


e{q) = 


h-h 

h-bi 


^2 ~^2 


h -bs 


6x1 


(Eq. 4.11) 


To correct this error an iterative method must be applied. In [HENA97], two 
different iteration methods were tested in simulation namely; Gradient Descent and 
Gauss-Newton. The Gradient descent method linearly converged, but needed dozens of 
steps to arrive at a satisfactory result. The Gauss-Newton method converged 
quadratically and needed less iteration. Based on this result, Gauss-Newton was used for 
this thesis. 
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For the Quaternion Attitude Estimation Filter the error correction vector which 
approximates Gauss-Newton’s iteration formula [MCGH63] is given in Eq. 4.12; 




(Eq. 4.12) 


where the X matrix is given by 


vT. — 

Ay - 


dyi 


(Eq. 4.13) 


4x6 


Appendix A presents the derivation of the X matrix given in Eq. 4.13. 
The least-squares criterion function is given by 


lx6£(q)6x\ 


(Eq. 4.14) 


where 


£iqf = [(^1 -hi)(h2-h2)(h3-h^Xhi-b^ih -^ 2)(4 
Substituting Eq. 4.15 into Eq. 4.14 yields 

Hqhxl =[(^\ -hif+(h2-h2f+{h3-h^f +(fcl -hf+ib2-b2f+{h-b3)^]m 4.16) 


The gradient of the error criterion function is defined as; 

:\T 


/' 


7(^(q) = 


d<p d(l> dcp d(j> 


■ 2X 4x6£i.q)6x\ 


(Eq. 4.17) 


The derivation of this gradient is given in Appendix B. 
Substituting Eq. 4.17 into Eq. 4.12 yields 


(Eq. 4.18) 


= -X''x]'‘^X 4 V( 9 ) 6 rf 


(Eq. 4.19) 
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The in Figure 4 represents the output from the top-half of the filter, and it will 
be used to correct the computed rate quaternion derivation, q. To find the output of the 
top-half of the filter the error correction vector, A^, must be multiplied by a constant 
gain, k 

= kh.q (Eq. 4.20) 

A scalar multiplier, a, is used when dealing with data corrupted by noise 

a = Mt (Eq.4.21) 

1. Linearization 

^q full is the correction to q based upon the Gauss-Newton iteration formula. 
Assuming there is no measurement noise, and that the computation of Aq y„/; is exact, it 
follows that [MCGH98C] 

^^4x1 = ^true (Eq. 4.22) 
^Qfull =<ltrue-^ (Eq.4.23) 

The signal flow diagram (SFG) representing this linearized system is presented in 
Figure 6. 
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Applying Mason’s formula [KU095] to Figure 6 to obtain the transfer function 
produces 


q kp ^ + p ^ p ^(l + kp 

itrue \ + kp ^ \-\-kp ^ 


(Eq. 4.24) 


thus 


^ = P ^^tme = (Itrue (Eq. 4.25) 

This result shows that regardless of the value of k, if nj and n 2 are zero, then 
q = qjj.^g. This is as expected for a complimentary filter. 

2. Response to Initial Condition Error 

Suppose the sensors are static and q is not correctly initialized then 

q,rue =(1000) (Eq. 4.26) 

^o = (ia;,ayaz) (Eq.4.27) 

If the noise sources and n 2 are both zero then the SFG can be redrawn as in Figure 7 


^true ^ ^ 



Figure 7: SFG for Initial Condition Error Analysis 

From Eq. 4.23 

-^X -ay -a^) (Eq. 4.28) 


Since the first component did not change, q will have the form 


q = (lxyz) 


(Eq. 4.29) 
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Using Eq. 4.27, the transform domain SFG for the scalar x is as follows 


a. 



Figure 8: Transform Domain SFG for x(s) 

From this SFG, and application of Mason’s formula it follows that [MCGH98B] 


X(s) 5 ' 1 

dx l + ks~^ s + k 


(Eq. 4.30) 


and therefore 


x{t) = d^e~^^ (Eq.4.31) 

Equivalent results apply for y{t) and £(/). Thus, any transient errors in q 
resulting from erroneous input from the magnetometer and the accelerometer will persist 
for a time inversely proportional to k. Specifically if 

^Aq=\ (Eq.4.32) 

then for any disturbance , the resulting error in the x component of q will be 


-t 

= (Eq.4.33) 

This error will be reduced to 37% of the initial value by time t = x^q . This 
shows that the filter must use a big k value. On the other hand, if the maneuver time 
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constant is ^maneuver ’ required that x^q be much larger than this value to suppress 
maneuver noise. Therefore, the qualitative requirement becomes 

-- »k»— (Eq.4.34) 

'^maneuver ^bias 

and the constraint for At [MCGH98C] is 

Ar < I (Eq. 4.35) 

k 

Reasonable values for At and k can be further adjusted by experimental means. 

3. Rate Sensor Bias Correction 

Drift is the tendency of sensor output to change over time with no change in 
sensor input and causes relentlessly increasing orientation measurement errors. This 
error is sometimes termed a bias. The integration of a bias-ridden angular rate signal will 
cause a steady build-up of error over time. This leads to an incorrect estimation of the 
body orientation relative to the Earth-fixed coordinate system. Angular rate sensor biases 
typically change unpredictably over time, making a simple, complete compensation 
impossible [ROBE97]. 

The filter takes the angular rate sensor readings and computes the bias-corrected 
output, . To find the bias-corrected output, the filter must estimate the bias error and 
use this estimate to correct subsequent measurements. Discrete low pass filter theory 
provides a method for obtaining a rate bias estimate [KU095]. 

The quaternion filter bias estimation is based on Shallow Water AUV Navigation 
System (SANS) experience [ROBE97]. The approach taken to deal with this problem in 
SANS was to estimate an initial value for the bias by averaging the rate sensor output 
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before beginning maneuvering and then tracking the time-varying drift with a very long 
time constant low pass filter [MCGH98D]. The signal flow graph for qmeasured given 
in Figure 9. 


^true Q measured 



Figure 9: Bias Estimation Filter 

Evidently 


^measured 
4 true 


l + P'^^bias 


(Eq. 4.36) 


Thus, with the addition of bias estimation, the quaternion filter is no longer a 
complimentary filter since constant rates of rotation are eliminated. This effect can be 
minimized by applying the constraint 

^ » hias (Eq- 4.37) 

If k is too large, then the filter may become unstable or too much maneuver induced error 
may appear in q [MCGH98D]. 

The computed quaternion derivative, q, is computed by the lower left-hand 
portion of the filter. Figure 10 [HENA97]. The expression for^ is as given in Eq. 3.84. 


. 1 

q = ^q CO 


(Eq. 4.38) 
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Estimated Bias 


% % '■p 



Figure 10: Lower Left Part of Quaternion Attitude Estimation Filter 
The bias-corrected angular rate quaternion is corrected for the estimated bias (pb 

qb rb). The corrected quaternion derivative q is computed by 

q = q-qe (Eq. 4.39) 

The corrected quaternion derivative is then numerically integrated and 
normalized. The resulting numerical integration equation becomes 

qn+i=qn+\^n^coAt + k[x^xY\^e(q^)At (Eq. 4.40) 

The normalized result is the estimated orientation quaternion, q, which yields the 
next approximation. This approximated orientation quaternion can be used for the 
graphical representations of a tracked object in a virtual environment. 

D. WHITE NOISE EFFECTS 

White noise can be represented by a squence of statiscally independent numbers. 
The white noise effects on the filter for the Gauss-Newton method were tested by using a 
simulation program written in ANSI Common Lisp [HENA97]. A 6D white noise vector 
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was added to the measurement vector and a different white noise sample was used for 
every iteration. A scalar multiplier, a , was used to decrease the white noise effects on 
the filter. The modified filter with the white noise is shown in Figure 11. 


Magnetometer White Noise 



Figure 11: Modifled Filter with White Noise 

This modification is only made for testing the white noise effects on the filter; in 

real-time implementations this white noise must assumed to be in the measurements from 
the sensors. The filter responded to the simulated white noise in a corrective manner and 
the results were very close to the results with perfect noisless inputs. 

Two important results were obtained from this test. The first one was that 
reducing a improves results in the long term, but takes longer to converge. The second 
result was that for tracking motion, the optimal a depends on the rate of motion and the 
noise level. The simulation results without white noise and with white noise are 
presented in Figure 12 and Figure 13 respectively. The Figure 12 shows an exponential 
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Figure 13:10 Degree Offset, a=0.1, At=0.1, With Gaussian White Noise 
Noise Standard Deviation=0.57 Degree 






convergence. This result proves the value of the hnearization theory and the error 
convergence formula given in Eq. 4.33. 

E. SUMMARY 

The Quaternion Attitude Estimation Filter offers significant improvements over 
filters using Euler angles. Inputs are obtained from a 3-axis accelerometer, a 3-axis 
angular rate sensor and a 3-axis magnetometer. No singularities or divide by zero errors 
exist for any orientation and no trigonometric functions are required. Using unit 
quaternions makes calculating inverses simple and provides an increase in computational 
efficiency over matrix multiplication. 

The next chapter presents the hardware configuration of the quaternion attitude 
estimation filter system. 
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V. SYSTEM HARDWARE CONFIGURATION 

A. INTRODUCTION 

The hardware components of the quaternion attitude estimation filter system are 
configured for testing purposes. These components consist of a sensor block which holds 
a 3-axis magnetometer, a 3-axis accelerometer and a 3-axis rate sensor, a 12 VDC power 
source, an I/O connector, a 16-bit analog to digital (A/D) converter and a PC, Figure 14. 

I/O Connector 



Figure 14: System Hardware Configuration 

The power source supplies 12 VDC power for the sensor block. The ribbon 

cables, that carry actual output voltages from the sensors, are connected to the separate 
analog input charmels on the I/O connector via a breakout header. The A/D converter 
card is physically in the PC and receives data from the sensors via the I/O connector. 
This chapter will summarize the hardware configuration and the main hardware 
component capabilities used in the quaternion attitude estimation filter system. 
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B. HARDWARE DESCRIPTION 


1. Sensor Block 

The sensor block contains three individual sensors. The main purpose of this 
sensor block is to implement a small 9-axis sensor system. The sensor block contains a 
3-axis magnetometer, a 3-axis accelerometer, and a 3-axis rate sensor. Figure 15. This 
sensor block was constructed by [MCKI98]. 



Figure 15: The Sensor Block [MCKI98] 


The sensor block is covered with foam. The foam covering provides shock 
protection and a stable temperature environment for the sensors. The measurements of 
this package with foam coverage are 10.7 x 5 x 3.7 cm, and without foam coverage are 
9.1 X 3.9 X 2 cm. The size of the sensor block is very small, but with smaller sensors, a 
smaller sensor block can be constructed. 

The outputs from this sensor block are connected to the I/O connector by a ribbon 
cable via a breakout header. The DC power supply is also connected to this breakout 
header. 

The 3-axis magnetometer is a Honeywell HMC2003 type magnetic sensor. Figure 
16. The HMC2003 is a very small hybrid chip and can detect magnetic fields of less than 
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40 microgauss about three axes. Some characteristics of HMC2003 are given in Table 1 
[HONE98]. 



Figure 16: Honeywell HMC2003 3-axis Magnetic Sensor Hybird [HONE98] 


Characteristic 

Range 

Units 

Supply Voltage 

6-15 

VDC 

Field Range 

-2-2 

gauss 

Output Voltage 

0.5-4.5 

V 

Resolution 

40 

\i gauss 

Bandwidth 

1 

KHz 

NuU Field Output 

2.3-2.7 

V 


Table 1: 3-axis Magnetometer Specifications 


The 3-axis accelerometer is a Crossbow Technology Inc. CXL04M3 type 3-axis 
accelerometer, Figure 17. The CXL04M3 can measure accelerations about three axes. 
Characteristics of CXL04M3 are given in Table 2 [CROS98]. 



Figure 17: Crossbow CXL04M3 3-axis Accelerometer [CROS98] 
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Characteristic 

Range 

Units 

Supply Voltage 

+ 5 

VDC 

Span Range 

±4 ± 5% 

G 

Output Voltage 

0-5 

V 

Resolution 

5 

mGrms 

Bandwidth 

DC-100±5% 

Hz 

Sensitivity 

500 ± 5% 

mV/G 

Zero G Output 

2.5 ±0.1 

V 


Table 2: 3-axis Accelerometer Speciflcations 


The 3-axis rate sensor is a Tokin America Inc. CG16D0 type solid state rate 
sensor. The CG16D0 can measure the angular velocities about three axes. Some 
characteristics of CG16D0 are given in Table 3 [TOKI98]. 


Characteristic 

Range 

Units 

Supply Voltage 

+ 5 

VDC 

Detect Range 

±90 

deg/sec 

Output Voltage 

0-5 

V 

Bandwidth 

100 

Hz 

Sensitivity 

1.1 ±20% 

mV/deg/sec 

Reference Voltage Output 

2.4 ± 10% 

V 


Table 3: 3-axis Angular Rate Sensor Specifications 

The sensor block can support 100 Hz sampling rates. The magnetometer has a 
1 KHz bandwidth. 
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2. A/D Converter 

The A/D converter is a National Instruments Corporation® [NATI98] PCI-MIO- 
16XE-50 data acquisition (DAQ) card. Figure 18. It is physically inserted into the PCI 
slot of a PC motherboard. The PCI-MIO-16XE-50 is a 16-bit A/D converter, it has 16 
single-ended or 8 double-ended analog input channels. Its maximum sampling rate is 
20 K samples/sec. The recommended warm up time is 15 minutes. 



Figure 18: National Ins. PCI-MIO-16XE-50 Data Acquisition Card [NATI98] 

It can measure analog input voltages between OV - lOV (single sided) and ±10V 

(double sided). Sensor output voltage connections with A/D converter channels are 
shown in Table 4. 



Angul 

iar Rate Sensor 

Accelerometer 

Magnetometer 1 

X 

y 

z 

X 

y 

z 

X 

y 

z 

p. 

q. 

r 

hi 

h2 

h3 

bl 

b2 

b3 

Channel No 

0 

1 

2 

3 

4 

5 

6 

7 

8 

Pin No 

68 

33 

65 

30 

28 

60 

25 

57 

34 

Ground 

67 

29 

64 1 


Table 4: Sensor Channel Connections 


3. Other Components 

The computer used in the research described in this thesis contains a 333 Mhz 
Intel Pentium 11 CPU and 64 MB of RAM. The operating system is Microsoft Windows 
95™. 
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Figure 19: National Ins. SCB68 I/O Connection Board [NATI98] 

The I/O connector is a National Instruments Corporation® [NATI98] SCB68 type 

I/O connection board, Figure 19. This I/O connector connects the output ribbon cable 

from the breakout header with the associated channels of the A/D converter. 

C. SUMMARY 

The hardware configuration described in this chapter was used for the quaternion 
attitude estimation filter system. For further information about the hardware components 
refer to the related product catalogs and the web sites of the manufacturers. The 
estimated equipment and material cost of the hardware except the PC is about $3,500. 
The next chapter presents the software design documentation of the quaternion attitude 


estimation filter. 




VI. SOFTWARE DEVELOPMENT 


A. INTRODUCTION 

The Unified Modeling Language (UML) is a language for expressing the 
constructs and relationships of complex systems. A critical aspect of real-time systems is 
how time itself is handled. The design of a real-time system must identify the timing 
requirements of the system and ensure that the system performance is both correct and 
timely [DOUG98]. UML is particularly well suited for designing real-time embedded 
systems. 

The features, concepts and visual notations used in UML are explained in 
[DOUG98]. During the software development process of the quaternion attitude 
estimation filter the Rational Rose 4.0 software design tool [RATI98] was used to 
implement the system in UML. 

The steps in software development are system requirements analysis, system 
analysis, system design and implementation [DOUG98]. This chapter will present these 
steps for the quaternion attitude estimation filter software development. 

B. SYSTEM REQUIREMENTS ANALYSIS 

The quaternion attitude estimation filter (QAEF) is a complementary filter 
designed to track human limb segments through all orientations as part of an inertial 
tracking system The QAEF uses three different sensors to obtain the information about 
the orientations of a tracked object. These sensors are a 3-axis accelerometer, a 3-axis 
angular rate sensor and a 3-axis magnetometer. The system gets these sensor readings 
through an I/O connector by using an analog to digital (A/D) converter. The system 
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samples and filters these data to produce an estimated orientation of the tracked object in 
the quaternion form. 

The QAEF will filter the data from only one set of sensors, but is intended to be 
the building block for a body suit using several sensor blocks. The system use case 
diagram is presented in Figure 20. 



Figure 20: Use Case Diagram 



Figure 21: Context Diagram 

The system’s interaction with the actor objects is shown in the context diagram. 
Figure 21. A sequence diagram which shows the sequence of messages between objects 
is presented in Figure 22. The scenario diagram is the same as the sequence diagram. 
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because the system has a continuous behavior and there are no external eveiits related to 
the system. 


S Block : Sensors 


AD Converter: Converter 


mv Sampler: 


Mv Filter: Filter 


Quat Filter: Qaef 



Sampler 






1: Get data 




1 

^ 2 : Update buffer 

1 

1 

1 

1 

1 

1 

1 

< 1 

1 


1 

1 

3: Get new sample , 

1 ^ 

1^ 

, 4: Request neur data 

JUei: _1 

j 

L L 




' 5: Transfer data , 


* 6: Sample data 


7: Correct bias error 


, 8: Filter data 


I 9: Update result 


1 ] 


AUV: Simulation 
Program 


ID: Get result 




Figure 22: Sequence Diagram 
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The system will target a sampling rate between 40 Hz and 100 Hz, and an update 
rate of at least 30 Hz for real-time applications. The use cases and the system goals are 
presented in Table 5 and Table 6 respectively. 


Goall 

System has to get data from the sensors and convert it into digital 
data with a sampling rate of at least 40 Hz 

Goal 2 

System has to have at least a 30 Hz update rate at the end of the 
filtering process. 


Table 5: System Goals 


Use Case 

(Ul) Get raw data 

Actor 

Sensors 

Goal 

Traced to Goal 1 

Preconditions 

Sensors and A/D Converter are running 

Description 

Object rotates or keeps its rotation. System will always get the 
readings from the sensors, convert this data into digital data 

Sub Use Case 

None 


None 

Activities 

Get the readings and write into a buffer 

Postconditions 

Continue getting data 



Use Case 

(U2) Filter data 

Actor 

Simulator program 

Goal 

Traced to Goal 2 

Preconditions 

Sensors, A/D Converter and system are running 

Description 

System will get the data from buffer and produce an estimated 
orientation in the quaternion form by filtering the data 

Sub Use Case 

None 

|^^§3S33lilHi 

None 

Activities 

Read data from the buffer, apply filtering algorithms and calculate an 
estimated orientation 

Postconditions 

Continue filtering processes 


Table 6: System Use Cases 


The major functions performed by the QAEF are data acquisition, data filtering, 
and updating, displaying and sending the estimated results. The actual system functions 
are given in Table 7. 
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System Functions 

Rank 

Use case 




Rl. Data Acquisition Functions 



Rl.l Get sensor readings 

Essential 

U1 

R1.2 Write data into the buffer 

Essential 

U1 

R1.3 Read data from the buffer 

Essential 

U1,U2 

R2. Data Filtering Functions 



R2.1 Average the data 

Essential 

U2 

R2.2 Calculate me^urement vector 

Essential 

U2 

R2.3 Calculate computed measurement vector 

Essential 

U2 

R2.4 Calculate error 

Essential 

U2 

R2.5 Calculate bias error 

Essential 

U2 

R2.6 Calculate X-matrix 

Essential 

U2 

R2.7 Calculate Gauss-Newton iteration 

Essential 

U2 

R2.8 Calculate numerical integration 

Essential 

U2 

R2.9 Convert data into digital 

Essential 

U2 

R3. Matrix Operations 



R3.1 Multiply 

Essential 

U2 

R3.2 Invert 

Essential 

U2 

R3.3 Transpose 

Essential 

U2 

R3.4 Convert to quaternion 

Essential 

U2 

R3.5 Print 

Essential 

U2 

R4. Quaternion Operations 



R4.1 Add 

Essential 

U2 

R4.2 Subtract 

Essential 

U2 


Table 7: System Functions 
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R4.3 Quaternion multiplication 

Essential 

U2 

R4.4 Rotation 

Essential 

U2 

R4.5 Dot product 

Essential 

U2 

R4.6 Derivative 

Essential 

U2 

R4.7 Normalize 

Essential 

U2 

R4.8 Print 

Essential 

U2 

R5. Output Functions 



R5.1 Print to the screen 

Desired 

U2 

R5.2 Write into a file 

Optional 

U2 

R5.3 Write into a buffer 

Optional 

U2 


Table 7: System Functions (Cont’d) 


C. SYSTEM ANALYSIS 

The QAEF system boundary is limited by the A/D converter. The actual filter 
apphcation will pass the estimated results to a 3D simulation or to a tracking program. 
For this research the system will print the results to the screen and will write them into a 
file. The system behavior is continuous and will implement a Proportional Integral 
Derivative (PID) control loop. The system will run on a PC under the Microsoft 
Windows 95™ operating system. The identified objects are shown in the object diagram. 
Figure 23. The system state chart showing the general system behavior is in presented in 
Figure 24. 
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Figure 23: Object Diagram 



Figure 24: System Statechart 

The system functions are distributed to the identified classes. The associations 
and the relations of these classes are determined fi’om the object diagram and from the 
sequence diagram to create the class diagram for the system. The class diagram is 
presented in Figure 25. 
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D. SYSTEM DESIGN 


Design details the largest scale software structures, such as subsystems, packages 
and tasks, and specifies the internal primitive data structures and algorithms within 
individual classes [DOUG98]. The QAEF system hardware configuration is explained in 
Chapter V. The deployment diagram for the QAEF hardware is presented in Figure 26. 

The tasks are identified as “Get Sensor Readings”, “Sample Data” and ‘Eilter 
Data”. The “Get Sensor Readings” task gets sensor readings and writes these readings 
into the double buffer. The Filter Data task calls its server task, the Sample Data task, to 
get the current sample readings. The Filter Data task receives these samples via an array 
and then filters the data to produce an estimated output result. Sample Data task sends a 
data transfer request signal to the Get Sensor Readings task, and then the Get Sensor 
Readings task updates the transfer buffer with half of the double buffer. The Filter Data 
task should wait for buffer updating and the sampling data processes. The data 
transferring process from the double buffer to the transfer buffer will have a timeout. 
During this period, the Filter Data task will be waiting for the sampled data and for the 
Get Sensor Readings task to finish this transfer. If for some reason this process takes too 
much time, then the Filter Data task should continue the data filtering process with the 
current updated data. The length of the timeout is changeable and is dependent on the 
speed of the filtering and the acquisition processes. The timeout value is calculated to be 
1/60 of a second, but it can be changed during the system tests. 
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Figure 26: System Deployment Diagram 
The QAEF system has two subsystems namely the Filtering Subsystem and the 


Acquisition Subsystem. The Filtering Subsystem is responsible for data sampling and for 
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data filtering. This subsystem is composed of the Filter, Sampler, Quaternion, and 
Matrix classes in the Data Filtering package, Figure 27. 



Figure 27: Filtering Subsystem Structure 

The active objects in Data Filtering package are the Filter and Sampler. The operations 
of these active objects in the Filtering Subsystem are shown in Figure 28. The Filter is 
responsible for the calculations in the filtering process. The Filter object requests data 
from the Sampler. Upon receiving the sampled data, it calculates the bias correction (the 
Filter object must calculate the initial bias correction for the first step only). After 
applying this bias error to the sampled data, it starts the calculation process to find an 
estimated result. The result will be printed on the screen and will be written into a file. 
The Filter object state model is presented in Figure 29. 

The Sampler is responsible for initMizing the calibration values, getting data 
from the converter, averaging this data, and providing this sampled data to the Filter 
along with the time interval of the samples. The calibration values are the zero level 
voltages for the sensor readings. These values can either be found by the Sampler or can 
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be hard coded. The Sampler object gets data from the converter when requested by the 
Filter. The Sampler samples the data by averaging, and then sends the samples 
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Figure 28: Filtering Subsystem Operations 
and the time interval to the Filter. The Sampler will wait for the data transmission from 

the double buffer into the transfer buffer. If this transmission exceeds the transmission 

time, then a timeout occurs. The state model for the Sampler is presented in Figure 30. 
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Figure 29: Filter Object State Model 



Figure 30: Sampler Object State Model 


73 














The Quaternion class supports the system with quaternion operations and with 
quaternion arithmetic as explained in Chapter HI. This class could support any 
simulation or tracking program using quaternions. 

The Matrix class supports the system with necessary matrix operations. These 
operations are used in the filtering process and are special for this design. It also has the 
ability to make combinations of and conversions between quaternions and matrices. 

The Acquisition Subsystem is responsible for getting data from the sensors and 
for updating the double buffer with this data. This subsystem is composed of the 
Converter class in the Data Acquisition package, as depicted in Figure 31. 



package Data Acquisition 



Converter 



Figure 31: Acquisition Subsystem Structure 
The Converter is an active object. The Converter class provides an interface between the 

system and the converter hardware. The Converter is responsible for initiating and 

cahbrating the A/D converter, for monitoring the data collection process, for updating the 

double buffer with new data, and for transferring halves of the double buffer into the 

transfer buffer. The Converter communicates with the Sampler during this transfer 

process. It checks for the timeout condition and monitors all events for any possible error 

in data acquisition, such as buffer overflow and buffer overwrite. The Converter 

concurrently checks the channels for new data, and checks for any requests from the 

Sampler. The operations of the Converter in the Data Acquisition Subsystem are shown 

in Figure 32. 
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Figure 32: Acquisition Subsystem Operations 
The Converter updates the double buffer when new data is received and it transfers the 


data into the transfer buffer. The state model for the Converter is presented in Figure 33. 



Figure 33: Converter Object State Model 
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E. SYSTEM IMPLEMENTATION 


The system was implemented using the C-H- programming language and the 
Visual C++ 5.0 compiler. The attribute and the variable names given are the same as 
those used to define the Quaternion Attitude Estimation Filter in Chapter IV. The 
attributes and the variables are commented and the functions are explained along with 
their parameters in the function headers. Terminology consistency between the 
documentation and the code were strived for throughout implementation. 

The Gauss-Newton method and the filtering equations are implemented in the 
Filter class as they are presented in Chapter IV. Operator overloading is used in both the 
Quaternion class and the Matrix class to clarify the source code and to provide for ease of 
use. The Quaternion class has the ability to perform every quaternion operation 
explained in Chapter HI. Eyery class has the ability to print its pre-specified attributes by 
overloading the stream insertion operator. 

Utility functions to write into and read from a file are provided for the user. All 
classes have default constructors, but the user has the opportunity to initialize the objects 
by reading the initial values from a configuration file. 

A function library, nidaq32.1ib, is used to program the converter and these 
functions are explained with their parameters in [NATI98A]. The converter has two 
types of data acquisition, single buffered and double buffered. Single buffered operations 
are relatively simple to implement, and can usually take advantage of the full hardware 
speed of data acquisition device. However, more sophisticated apphcations involving 
larger amounts of data input at higher rates require more advanced techniques. One such 
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technique is double buffering. This allows continuous, uninterrupted input of large 
amounts of data [NATI98B]. In this implementation, double buffering was chosen. 

The converter is configured to work in bipolar mode, ±10 V, with board gain 2. 
In this configuration, the digital ranges for the actual voltages will be between -32768 
and 32767. These values are used to initialize the converter object. 

F. SUMMARY 

The QAEF software is designed to track motion in all orientations in real time. 
The UML and its visual notations were used to analyze and to design it. The software is 
written in C++, with the Visual C++ 5.0 compiler, for use on an IBM-compatible 
processor. This chapter presented a general overview of the software development 
process. The source code is presented in Appendix C. The test plans and the actual 
testing of a system are the final steps in the software development process. The next 
chapter will present the testing methodology and the test results of the QAEF system 
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Vn. SYSTEM TESTING 


A. INTRODUCTION 

System testing is the final step in software and hardware development. The 
quantitative tests focused on the software and the accuracy of the system. This chapter 
presents system calibration, testing methodology and the test results. 

B. SYSTEM CALIBRATION 

To initialize the system three values must be provided to the QAEF object, 
namely converter number, buffer depth and a constant gain value for k. The converter 
number specifies the converter to use when there is more than one converter. The buffer 
depth indirectly determines update rate. Up to the point of buffer overwrite errors, small 
buffer depths will provide faster update rates. The constant gain value k is discussed in 
Chapter IV. Throughout the quantitative tests, k was set to 3 and a was kept less than 1. 

Most of the calibration is done within the sampler object. The sampler object has 
a self-calibrating function namely findZeroVoltages. This function finds the zero level 
voltages for angular rate sensors and accelerometers. The sensor block must be sitting on 
a level surface for several milliseconds to find these zero level voltages. Hard coded 
values may also be used. These hard coded values can be determined by taking the 
average values of the maximum and the minimum readings of each sensor in the sensor 
block. The self-calibration process can be enabled or disabled. When the self-cahbration 
is enabled the findZeroVoltages function will initialize the sampler, otherwise the 
sampler will initialize itself with hard coded values. In actual tests, initializing the Earth 
magnetic field to the first reading from magnetometer obtained during calibration 
provided better results and simplified the initialization process. 
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All calibration of the A/D converter is done automatically by the converter object. 
The parameters to calibrate the A/D converter are chosen to get the best performance 
from the A/D converter. These parameters can easily be changed using the parameters 
discussed in [NATI98A] and making changes to the converter object. 

C. TESTING METHODOLOGY 

The sensor block was mounted on the tilt table using a non-ferrous extension, 
which has approximately the length of a human forearm. The tilt table was programmed 
to rotate 45 degrees at a rate of 10 degrees per second on every axis for a series of roll, 
pitch and yaw tests. After initializing the system, three rotations were completed by the 
tilt table in each test. During each test, a 15 to 20 second stabilization period followed 
each movement. The scale factors and buffer depth values were adjusted to obtain the 
desired update rate and accuracy. 

D. TEST RESULTS 

The following results were obtained using the hardware and software described in 
Chapters V and VI. To achieve an update rate of 55 Hz, the graphical representation of 
orientation was disabled. The update rate with the graphical orientation representation 
and a single processor was approximately 15 Hz. The roll, pitch, and yaw test results are 
presented in Figures 34, 35, and 36 respectively. 

Estimated orientation was within 1 degree of actual steady orientation throughout 
the tests. The smoothness of the graphs indicates excellent dynamic response. The small 
impulses observed at the starting points of motion were hypothesized to be linear 
acceleration effects exaggerated by a whipping motion of the non-ferrous extension 
block. It is expected that adjusting the filter scale factors and gain values will reduce this 
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0 50 100 150 

Figure 35: 45 Degree Pitch Test, 10 deg/sec, a = 0.054,55 Hz. 
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Figure 36: 45 Degree Yaw Test, 10 deg/sec, a = 0.054,55 Hz. 
effect and improve the overall accuracy and dynamic response. The transition times 

observed in the plots are around 4.5-5 seconds as expected for a 10-degree per second 

rotation rate to 45 degrees. In qualitative tests, the system was able to track all 

orientations, including those in which pitch equaled 90 degrees; the same orientations 

normally cause singularities in Euler angle filters. The qualitative tests also show that the 

system could easily be combined with a simulation program and track motion in real 

time. 

E. SUMMARY 

This chapter has described the system calibration, the system testing methodology 
and the qualitative and quantitative dynamic tilt table test results. These tests show that 
the system’s lag is very low, and the accuracy is within 1 degree when a proper 
calibration is completed. The prototype system update rate can be adjusted between 15 
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and 55 Hz. It appears to be capable of tracking human motion in real time. The final 
chapter of this thesis will review this research, reach some conclusions and make 
recommendations for future work. 
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Vin. SUMMARY AND CONCLUSIONS 


A. SUMMARY 

Several techniques exist to track human motion and many research efforts have 
addressed body tracking systems and orientation filters. The most commonly used 
method for defining orientation is Euler angles. Motion tracking systems using Euler 
angles are not capable of tracking objects in all orientations due to gimbal lock 
singularities. An alternative method, the use of unit quaternions, has been gaining 
popularity in the graphics community since mid 80’s. A quaternion based attitude 
estimation filter has been proposed to overcome these singularities. This thesis described 
the development and testing of a prototype inertial tracking system based on a quaternion 
attitude estimation filter. Test results indicate that human motions could be efficiently 
tracked with this system. 

B. CONCLUSIONS 

This research continues that begun in [HENA97]. The presented system is the 
first inertial body tracking system based on quaternions [DURL95, SKOP96]. Using unit 
quaternions prevented the singularities and increased computational efficiency. 

The system resolution is very good, and accuracy is within 1 degree. Robustness 
and registration are very good in a homogenous environment, but relatively strong 
magnetic fields will effect the system. Since the system is sourceless, it can be assumed 
that there is no limit to sociability in a very large homogenous environment. The 
prototype sensor is small and it is light. This makes the system user Mendly and easy to 
use. Smaller sensor blocks, wristwatch size, could be produced with smaller sensors in 
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the near future. The system was constructed using low cost, off the shelf components. 
System cost and size would be reduced with mass production. 

C. RECOMMENDATIONS FOR FUTURE WORK 

The system will be used as a part of the on going body suit project [ZYDA97] and 
could also be used in the NFS AUV project [YUN97]. The system and multiple sensor 
blocks can be combined with the quaternion human model [USTA99] for real time 
human motion tracking applications. Using multiple sensor blocks will decrease the 
system performance on a single processor system To prevent this, a multiprocessor 
architecture should be developed for full human body motion tracking applications. 
Tracking humans in large environments will require a wireless tracking system 
Developing a wireless tracking system architecture should be considered. 

The system has a self-cahbrating module or the scalar numbers can be hard coded 
for system calibration. An improved calibration procedure should be developed to 
achieve better results. The filter can be used with other types of sensors. Integrating 
better sensors into the system should be considered. 
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APPENDIX A. DERIVATION OF X MATRIX 


The X matrix is defined by (Eq. 4.13) as 


vT_ M 

' "L^qj 


(Eq.A.l) 


The elements of the 4x6 matrix come from the partial derivatives of the 
components of the computed measurement vector, y{q), given by (Eq. 4.8) 


y(q) = (q ^mq,q ^nq) 


(Eq. A.2) 


and as given in (Eq. 4.6) and (Eq. 4.7) 


C A 1 A 

h = q mq 


(Eq. A.3) 


b=q nq 


(Eq. A.4) 


Therefore, taking the partial derivative of this equation with respect to q ^, the 


result is 


dy o’ A-l A A-l A 

ckiQ dqQ 


(Eq. A.5) 


Applying the product rule to (Eq. A.5), it follows that 


dy dq ^ dq dq a a-1 ^ 

-z;:^mq+q m-—,—^nq^q n— 


(Eq. A.6) 


where 


= ( 1000 ) 


(Eq.A.7) 
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and 
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where the corresponding quaternion partial derivatives are 

dq 

-^ = ( 0100 ) 

m 

dq 

^ = ( 0010 ) 
dqi 

dq 

^ = (000 1 ) 
dq^ 

and, the partial derivatives of the inverse are given by 

dq-^ 

^ = (0 -10 0 ) 
dq\ 

^ = (00 - 10 ) 
dqi 

dq~^ 

^^ = (000 - 1 ) 
dqs 


(Eq. A. 8 ) 

(Eq.A.9) 

(Eq. A. 10) 

(Eq. A. 11) 

(Eq. A. 12) 

(Eq. A. 13) 

(Eq. A. 14) 

(Eq. A. 15) 

(Eq. A. 16) 

(Eq. A. 17) 
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The partial derivatives as defined in (Eq. A. 6 ), (Eq. A.9), (Eq. A. 10), and (Eq. 
A. 11) result in the partial derivatives of the m and n vectors with respect to 
and ^ 3 , respectively. 

Taking the results computed above, the X matrix can be constructed as follows 




dy dy dy 


(Eq. A.18) 


J6x4 


L^o 

Note that the partial derivatives are column vectors in the X matrix, and that the 
transpose of X is required when used in the filter. Thus, (Eq. A.18) becomes [HENA97] 
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4x6 


Then the transpose of the X matrix is 
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(Eq. A.19) 


(Eq. A.20) 
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APPENDIX B. DERIVATION OF GRADIENT OF THE ERROR 


CRITERION FUNCTION 

In the filter, the gradient of the error criterion function <l>{q), is given by (Eq.4.17) 

where <l>{q), is given by (Eq.4.14) 

«>(«)w = «(4 )''w«(«)6,i (Eq. B.2) 

In order to simplify the derivation of the gradient, consider the the ceise of one 
dimensional orientation vectors, where the measured vector is yo~(^l) 

A. 

calculated vector is y{q) = (hi). The error then becomes 

S(q) = h-h (Eq. B.3) 

and 


= = = hi -Ihihi + hi 


(Eq. B.4) 


Taking the partial derivative of (Eq.B.4) with respect to qQ yields 


A. A. ^ A 

-^ = 2hi-^-2h^^ = 2^(/ii -hi) = 2-^£{q) 
dqo dqQ dq^ dq^ dq^ 


(Eq. B.5) 


Likewise, for the partial derivatives with respect to ^ 1,^2 > and ^3 [HENA97]. 


Thus, extending this result to 6 dimensional case and arranging the partial derivatives in 
matrix form yields the derivation of the gradient of the error criterion function. 
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The modeling error can be written 


-hi 

A 

^2—^2 


e{q) = 


/13 — 

h-bi 


h-h 


_4-^3 


(Eq. B.6) 


Based on this error, becomes 

(t>{q) = {hi-hif + {h2-h2f + {h 3 -h^f+i^ -hf + ih-b2? Hh-bsf (Eq. B. 7 ) 
Taking the partial derivative of (Eq. B. 7 ) with respect to qQ 

-^ = -^({hi-hif +(h2-h2)^ +{hs-h2)^ + (^-bif +{^-b2f +{^-b2f) (Eq.B.8) 
we obtain 
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(Eq. B. 9 ) 


Rows two through four are produced by taking the partial derivative of ^(^) with respect 
to qi,q2, and ^3. 
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Thus, 
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which can be rewritten [BACH97A] 

y^iq) = 2x'^e(q) 

where the transpose of the X matrix is as defined in Appendix A. 


(Eq. B.IO) 


(Eq. B.ll) 
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APPENDIX C. SOURCE CODE 


A. QAEF.H 


//★***★*★★★***★*★*********★*★*********************★***★**•*******★★**★ 
//★****************************************************************** 
// 

// File ; Qaef.h 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Header file for Qaef 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 
//***★***★*****★******★****★*****★★*★***★*********★*********★★******* 
//******★******★★*******★*★*★***********★★*********★★*******★*★★*** 

# i fnde f QAEF_H 

#define _QAEF H 


#include 
#include 
#include 
#include 
#include 


<iostream.h> 
<iomanip.h> 
"converter.h" 
"filter.h” 

"quaternion.h" 


class Qaef{ 
public: 

// default constructor 

Qaef (int converterNo ,unsigned int transferBufSize = 400, 
unsigned int kValue = 150); 

// destructor 
-Qaef(); 

// starts the filtering process 
double start(); 
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// stops the converter 
void stopO; 

// returns the estimated quaternion 
Quaternion getResultO; 

private: 

//AD converter 
Converter * qConverter; 

// Filter 
Filter * qFilter; 


}; 


#endif 

// end of file Qaef.h 
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B. QAEF.CPP 


//******★★★★*■*★**★★*★★★★*★*★*★*************★*★**★★*★★*•★★**★********★★ 
//***********★**★****★★******★*******★******★*★*★**★*********★*★**★*★ 
// 

//File : Qaef.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Qaef 

// - Initializes A/D converter and filter 

// - Returns estimated result 

// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assiamption : None 
// 

// Warnings during compliation : None 

//******★************************************************************ 

//*************************************★****★★****★***★************** 

#include <stdio.h> 

#include <stdlib.h> 
tinclude "qaef.h" 

#include ”converter.h" 

#include "filter.h" 

#include "quaternion.h" 


// 


// 


// 


Function: 

Qaef{) 

Return Value: 

None 

Parameters: 

None 

Description: 

Default constructor 


Qaef::Qaef(int deviceNo, unsigned int bufSize, unsigned int k ) 
:qConverter (new Converter(deviceNo,bufSize)), 
qFilter (new Filter(qConverter, k)) 

{ 

}//end Constructor 
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// - 

// Function: --Qaef () 

// Return Value: None 
// Parameters: None 

// Description: Destructor 

// - 

Qaef::-Qaef() 

{ 

qConverter->'-Converter {) ; 
qFilter->-'Filter {) ; 
cerr«"QAEF deleted'*«endl; 
}// end destructor 


// - 

// Function: start() 

// Return Value: None 
// Parameters: None 

// Description: Starts filtering process 

// --- 

double Qaef::start() 

{ 

qConverter->start(); 

return qFilter->start(); 

)//end start() 


// - 

// Function: getResultO 

// Return Value: Quaternion ~ estimated result 

// Parameters: None 

// Description: Returns the estimated result 

// - 

Quaternion Qaef::getResult() 

{ 

return qFilter->estimateRotation(); 

}// end getResultO 


// - 

// Function: stopO 

// Return Value: None 
// Parameters: None 

// Description: Stops the converter 

// - 

void Qaef::stop() 

{ 

qConverter->stop{); 

return; 

}// end stop 

//end of file Qaef.cpp 
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c 


FILTER.H 


//******★************************************************************ 
//******************************************************************* 
// 

// File : Filter.h 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Header file for Filter.cpp 

// - Filters data 

// - Estimates an result 

// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 

//******************************************************************* 

//******************************************************************* 

#ifndef _FILTER_H_ 

#define _FILTER H 


#include 
#include 
#include 
#include 
#include 
#include 


<iostream.h> 

<iomanip.h> 

"matrix.h" 

"quaternion.h" 
"sampler.h" 
"converter.h" 


class Filter{ 

// overloaded operator« 

friend ostream &operator«(ostream &,const Filter &); 
public: 

// default constructor 

Filter (Converter *, unsigned int k = 5); 

// destructor 
-Filter 0; 


// copy constructor 
Filter (const Filter &); 
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// starts filtering process 
double start(); 

// estimates a new rotation 
Quaternion estimateRotation(); 

private: 

Sampler *mySampler; //my server sampler 

Quaternion n, // Earth’s unit magnetic field 

m, // Earth’s unit gravity 

h, // local vertical from accelerometer 

b, // local magnetic field from magnetometer 

bw, // rate , from angular rate sensor 

deltaq, // Gauss Newton iteration 

qDot, // derivation of angular rate 

hHat, bHat, //in computed measurement vector 
qHat; // estimated quaternion 

// partial derivatives 
const Quaternion QHATZERO, 

QHATONE, ONEINV, 

QHATTWO, TWOINV, 

QHATTHREE, THREEINV; 

Matrix x, //X matrix 

xTranspose, //X transpose matrix 

errorqHat, // error q hat 

errorTranspose, // error transpose 

temp,tempi; // temp matrices for calculations 

int tau; //for bias error 

bool selfcalibration; // for Sampler to find zero level voltages 

double sampleWeight, // for bias error 

biasWeight, 

deltaT, // time between two sampling in seconds 

* biasError, // bias errors for angular rates 

* samples, // received readings from sampler 

kValue, // k value, will be a constant 

errorMagnetitude; // error's magnetitude 

// gets new sampled data from sampler 
void getNewSample (); 

// calculates initial bias error 
void initialBiasError(); 

// corrects bias error by low pass filtering 
void correctBiasError0; 

// sets the quaternions with new data 
void setElements(); 
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// calculates error 
void calculateError(); 

// creates X Transpose matrix 
void createXTranspose(); 

// calculates delta q quaternion 
void calculateDeltaQ(); 

// calculates qDot quaternion 
void calculateqDot{); 

// calculates final result 
void calculateResult{); 

// converts the final matrix solution in to quaternion 
void convertToDeltaq(Matrix &); 

// writes results into a file 
void logResults(); 


#endif 

// end of file Filter.h 
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FILTER.CPP 


//★***★★★★★★★★★★★★***★***★*★*★★★★★★★★***★*★**★★***★★★*★*★*★★★*★★*★*** 

//*★**★**********★**********★★*★*****★**★*★*★*★★★*★★★**★★★*★*★******* 

// 

// File : Filter.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Filter 
// - Filters data 
// - Estimates an result 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 
//★★***************************★★★★★*★★★*★**★*•***★*★*★***★*★*****★*** 
//*★★★*★★★★★**★★★★*★★★★★★*★★***★****★★★★★★★★★★★★★★***★★****★★*****★** 

#include <math.h> 
tinclude <string.h> 

#include <assert.h> 

#include <sys/timeb.h> 

#include <time.h> 

#include <fstream.h> 

#include <stdlib.h> 

#include ”filter.h” 

#include "matrix.h" 

#include "quaternion.h" 

#include "sampler.h" 

#include "util.h" 

ofstream fresults; 

// - 

// Function: Filter() 

// Return Value: None 
// Parameters: None 

// Description: Default constructor 

// - 

Filter::Filter (Converter * con, unsigned int kVal) 

:n (Quaternion ("n", 0 , 0 , 60 , -15)),//for Monterey, CA, in Euler foirm 
m(Quaternion ("m",0.0,0.0,0.0,-1.0)), 
h(Quaternion ("h")), 
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b (Quaternion ("b”) ) , 

bw(Quaternion("bw”)), 

deltaq(Quaternion ("delta q"))/ 

qDot(Quaternion ("q dot")), 

hHat(Quaternion ("h hat")), 

bHat(Quaternion ("b hat")), 

qHat(Quaternion ("Result",1.0,0.0,0.0,0.0)), 

QHATZERO(Quaternion("qhat zero",1.0,0.0,0.0,0.0)), 

QHATONE(Quaternion ("qhat one",0.0,1.0,0.0,0.0)), 

ONEINV(Quaternion ("qhat one inv",0.0,-1.0,0.0,0.0)), 

QHATTWO(Quaternion ("qhat two",0.0,0.0,1.0,0.0)), 

TWOINV(Quaternion ("qhat two inv",0.0,0.0,-1.0,0.0)), 

QHATTHREE(Quaternion ("qhat three",0.0,0.0,0.0,1.0)), 

THREEINV(Quaternion ("qhat three inv",0.0,0.0,0.0,-1.0)), 

X(Matrix ("X",6,4)), 

xTranspose(Matrix ("X Transpose",4,6)), 
errorqHat (Matrix ("error q hat",6,1)), 
errorTranspose(Matrix ("Error Transpose",1,6)), 
errorMagnetitude(0.0), 
temp (Matrix ("temp",4,4)), 
tempi (Matrix ("tempi",4,1)), 
tau(lOO), deltaT(O.l),kValue(kVal), 
sampleWeight(0.0), biasWeight(0.0), 

selfCalibration(true) // sampler will find zero level voltages 

{ 


writeFile(fresults,"FilterResults.dat"); 

mySampler = new Sampler(con); 

biasError = new double [3]; 
assert (biasError !=0); 

samples = new double [9]; 
assert (samples !=0); 

for (int i=0;i<3;i++){ 

biasError [i] = 0.0; 

} 

for (int k=0;k<9;k++){ 

samples [k] = 0.0; 

} 

qHat.normalizeO ; 

// find the Earth’s unit magnetic field , n 
Quaternion tt ("tt",0,1,0,0); // just a temp quaternion 
n=n.toQuaternionO ; // convert n to Quaternion 

n=n.rotation(tt); // rotate n with (0100) 

/ /cerr«n«endl ; 
qHat.normalize(); 

// will be set to b ‘ 

n. setQuatemion(0.0,0.79 ,0.0055 ,0.61); 

//n.normalize() ; 
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cout «m«endl; 

cout «"K = "«kValue«endl; 

cout«"initial ”«qHat«endl; 

cout <<’'Filter : Initialized ''«endl; 

}//end Constructor 


// - 

// Function: -'Filter () 

// Return Value: None 
// Parameters: None 

// Description: Destructor 

// - 

Filter::-Filter() 

{ 

cout «"deleting Filter"«endl; 
delete [] samples; 
delete [] biasError; 
fresults.close(); 
mySampler->-Sampler(); 

}//end destructor 


// - 

// Function: Filter(Filter &) 

// Return Value: None 
// Parameters: Filter 

// Description: Copy constructor 

// - 

Filter::Filter(const Filter ficF) 

{ 

cerr «"copy constructor NOT implemented"; 
}// end copy constructor 


// - 

// Function: start() 

// Return Value: delta t - double 

// Parameters: None 

// Description: Starts the filtering process/calculates the initial 

// bias error 

// - 

double Filter::start() 

{ 

cout « "\nFilter : Filtering started"«endl; 
mySampler->findZeroVoltages(selfCalibration); 

. getNewSample () ; 
initialBiasError() ; 
correctBiasError() ; 
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setElements () ; 

// sets Earth's magnetic field to the first reading 
n=b; 

calculateError{); 

createXTranspose{); 

calculateDeltaQ(); 

calculategDot(); 

calculateResult(); 

// logResults(); 

return deltaT; 

}//end start 0 


f f - 

// Function: 

// Return Value: 
// Parameters: 

// Description: 
// 

// - 


estimateRotation() 

qHat - quaternion - estimated result 
None 

Estimate new rotation, must call itself for 
continuous estimation 


Quaternion Filter::estimateRotation() 

{ 

getNewSample{); 


correctBiasError(); 


setElements(); 
calculateError(); 


createXTranspose(); 
calculateDeltaQ(); 
calculateqDot(); 
calculateResult(); 
// logResults 0; 


return qHat; 

}// end estimateRotation() 
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// - 

// Function: initialBiasError() 

// Return Value: None 

// Parameters: None 

// Description: Calculates initial bias errors and sets them 

// - 

void Filter::initialBiasError{) 

{ 

int biasNumber = tau/10; 

// check for alpha 

if (((delta? * kValue) < 1.0) && ((delta? * kValue) > 0.0)){ 
cerr « "Alpha is "<<(delta?*kValue)«endl; 

}else { 

cerr <<"WARNING Filter : Alpha is "« (delta?*kValue) 

« "delta? "«delta?«" K " «kValue«endl; 

} 

for (int i=0;i<biasNumber;i++){ 

biasError[0] += samples[0]/biasNumber; //angular rate p 

biasError[l] += samples[1]/biasNumber; //angular rate q 

biasError[2] += samples[2]/biasNumber; //angular rate r 

} 

biasError[0] = -biasError[0]; 
biasError[1] = -biasError[1]; 
biasError[2] = -biasError[2]; 

return; 

}// end initialBiasError() 


// - 

// Function: correctBiasError() 

// Return Value: None 
// Parameters: None 

// Description: For every reading corrects angular rates with the 

// initial bias error 

// - 

void Filter::correctBiasError0 

{ 

sampleWeight = delta?/tau; 

biasWeight = 1-sampleWeight; 


for (int i=0;i<3;i++){ 

samples[i] += (biasWeight * 

} 


biasError[i])-(sampleWeight 
* samples [i]); 


return; 

}// end correctBiasError() 
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// - 

// Function: getNewSample() 

// Return Value: None 
// Parameters: None 

// Description: Calls Sampler to get new averaged reading 

// --- 

void Filter::getNewSample() 

{ 

mySampler->getData(samples, deltaT); 

/* 

for (int k=0;k<9;k++){ 

if (k==0) cerr«"\nBody Rates [ 

if (k==3) cerr«"Accelerometer [ 

if (k==6) cerr«"Magnetometer [ 

cerr«samples [k]«" " ; 
if ((k==2) II (k=:=5)){ 

cerr«" ] " «endl ; 

} 

} 

cerr«" ] "«endl; 

*/ 

return; 

}// end getNewSample() 


// - 

// Function: setElements() 

// Return Value: None 
// Parameters: None 

// Description: set b,h,bw with new readings 

// - 

void Filter::setElements() 

{ 

//reset angular rates 

bw.setQuaternion(0.0, samples[0], samples[1], samples[2]); 
//reset accelerometer measurements 

h.setQuaternion(0.0,samples[3],samples[4],samples[5]); 
h.normalize () ; 

//reset magnetometer measurements 

b.setQuatemion(0.0, samples [6] , samples [7] , samples [8]) ; 
b.normalize () ; 

return; 

}// end setElements() 
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// - 

// Function: calculateError() 

// Return Value: None 

// Parameters: None 

// Description: Calculates error vector 

// - 

void Filter::calculateError() 

{ 

static Quaternion errH("error h"); 

static Quaternion errB("error b"); 

hHat = qHat.toBody(m); 

bHat = qHat.toBody(n); 

errH = hHat h; 

errB = bHat - b; 

errorMagnetitude = errH.dotProduct(errH) + errB.dotProduct(errB); 
errorqHat.insertColQuaternion ( (hHat-h) , (bHat-b) , 0); 

return; 

}// end calculateError() 


// - 

// Function: createXTranspose() 

// Return Value: None 

// Parameters: None 

// Description: Creates X Transpose matrix 

// - 

void Filter::createXTranspose() 

{ 

xTranspose.insertRowQuaternion(m.partialDerivative(qHat,QHATZERO, 
QHATZERO),n.partialDerivative(qHat,QHATZERO,QHATZERO) , 0); 
xTranspose.insertRowQuaternion(m.partialDerivative(qHat,QHATONE, 
ONEINV) ,n.partialDerivative(qHat,QHATONE,ONEINV) , 1); 
xTranspose.insertRowQuaternion(m.partialDerivative(qHat,QHATTWO, 
TWOINV) , n.partialDerivative (qHat, QHATTWO, TWOINV) , 2); 
xTranspose.insertRowQuaternion(m.partialDerivative(qHat,QHATTHREE 
,THREEINV) ,n.partialDerivative(qHat,QHATTHREE,THREEINV) , 3); 
return; 

}// end createXTranspose() 

// - 

// Function: calculateDeltaQ() 

// Return Value: None 
// Parameters: None 

It Description: Calculates delta q in matrix form 

// - 

void Filter::calculateDeltaQ() 

{ 

xTranspose.transpose(x); 
temp = (xTranspose * x); 
temp = temp.invert(); 

tempi = temp * (xTranspose^errorqHat); 

convertToDeltaq(tempi); 

return; 

}// end calculateDeltaQ() 
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// -- 

// Fiinction: convertToDeltaQ () 

// Return Value: None 

// Parameters: MAT - calculated delta q matrix 

// Description: Converts deltaQ 4x1 in to quaternion 

// -- 

void Filter::convertToDeltaq(Matrix & MAT) 

{ 

if ((MAT.getRowC)==4) && (MAT.getCol()==1)){ 
deltaq.setQuaternion(MAT.getElement(0,0),MAT.getElement(1,0), 


MAT.getElement(2,0),MAT.getElement(3,0)); 

deltaq = (-1 * kValue)*deltaq; 

} else { 

cerr «"Error in Filter: matrix can not" 

«" be converted into quaternion "«endl; 


} 


return; 

} // end convertToDeltaQ() 


// - 

// Function: calculateqDot() 

// Return Value: None 

// Parameters: None 

// Description: Calculates qDot quaternion 

// - 

void Filter::calculateqDot() 

{ 

qDot = 0.5 *(qHat * bw); 
return; 

}// end calculateqDot() 


// - 

// Function: calculateResult() 

// Return Value: None 
// Parameters: None 

// Description: Finds the estimated result 

// - 

void Filter::calculateResult0 

{ 

qHat = qHat + ((qDot * deltaT) + (deltaT * deltaq)); 
qHat.normalize(); 

return; 

}// end calculateResult() 
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// - 

If Function: calculateResult() 

// Return Value: None 
// Parameters: None 

// Description: Finds the estimated result 

// - 

void Filter::logResults() 

{ 

static int steps = 1; 

fresults«steps«'' "«qHat« ” Error = "«errorMagnetitude«endl ; 
steps++; 

return; 

} 

// - 

// Function: (friend) operator«() 

// Return Value: ostream object 
// Parameters: output - ostream object 

// q - Filter to print 

// Description: Prints the Filter in a form, should be written out 

// of class 

// - 

ostream &operator« (ostream ^output, const Filter &:q) 

{ 

output «'['«"Filter's members*'«']'«q.n«q.m«q.h<<q.b« 
q. bw«q. del taq«q. qDot«q. hHat« 
q. bHat«q. qHat«q. x«q. xTranspose« 
q. errorqHat«q. errorTranspose«endl; ; 

return output; 

}// end operator<< 

//end of file Filter.cpp 
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E, 


CONVERTER.H 


Z/******************************************************************* 

//★*★*****★★***★★★*********★*★★*★*★★*★★★*************:>:*★★******★***** 

// 

//File : Converter.h 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// “ Source file for Converter 
// - Receives data from sensors 
// - Updates the double buffer 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 
//***★★★*★*****★***************************************************** 
//*****************★********★**********★******★***********★********** 

#ifndef _CONVERTER_H_ 

#define _CONVERTER__H_ 

tinclude <iostream.h> 

#include <iomanip.h> 

#define M_PI 3.14159265358979323846 

#define GRAVITY 32.2185 //feet per second 


class Converter{ 

// overloaded operator« 

friend ostream &operator«(ostream const Converter &); 
public: 

// default constructor 

Converter (int deviceNum, unsigned int rows=300) ; 

// destructor 
-Converter() ; 

// starts the converter 
void start(); 
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// checks if half buffer is ready 
bool isHalfReady(); 

// transfers half of the double buffer 
void transferData(short *); 

// stops the converter 
void stopO; 

// passes the configuration data 

void getConfigData (unsigned short &,unsigned short &,int &,int Sc, 
double &); 


private: 


long bufferSize, 

// 

buffer size 

timeOut; 

// 

time out 

int deviceNumber; 

// 

device number 

short * channelVector, 

// 

channels 

* gainVector, 

// 

gains for channels 

* doubleBuffer; 

// 

double buffer 

double rate; 

// 

scan rate 

short status, 

// 

returned status of converter functions 

numberOfChannels, 

// 

number of channels 

halfReady, 

// 

half ready 

dagStop, 

// 

DAQ stop 

sampleT__Base, 

// 

sample time base 

scanT_Base, 

// 

scan time base 

units. 

// 

used units during scanning 

dBEnable, 

// 

flag, double buffer enable 

inputMode, 

// 

converter input mode 

inputRange, 

// 

input range 

polarity. 

// 

input polarity 

drive, 

// 

specifies drive 

gain; 

// 

gain for scanning 


// data points to tranfer to transfer buffer 
unsigned long pointsTransfer; 


unsigned short sainpleinterval, 
scaninterval, 
maxRange; 

bool ready; 

// initialize converter 
void initConverter(); 

// calibrates converter 
void calibrateO; 


// sample interval 
// scan interval 
// max reading range 

// flag to indicate half ready 
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// sets events 
void setEvents() ; 

// restarts the converter 
void restart(); 

// prints the initial values 
void printInitialValues(); 


#endif 

// end of file Converter.h 
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F. 


CONVERTER.CPP 


//*•*•****★★★*★★*★**★***★★*★**★★*★*★*******★★★★★*★★★★★*★**★★★******★*★★★★ 

//★★*★*★★★★*★*★*★*****★★**★★★★★**★★******★★***★★★*★★*★***★★★★★★*★*★★★★★ 

// 

// File : Converter.cpp 
// Author : ildeniz DUMAN 

// Project Name ; Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Converter class 
// - Receives data from sensors 
// - Updates the double buffer with this data 
// - Transmits the half buffer 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 

//★★★**★**★★★★****★****★★*★★★★★★★★**★★★*★★★*********★***★**★*****★**★ 

//***★*★★*★**★*★*****★*★*****★★*★***★**★**★**★★*★**★*★★*****★★*★***** 

#include <assert.h> 

#include <iomanip.h> 

#include ''converter.h" 

#include "nidaq.h" 

#include "nidaqcns.h" 

#include "util.h" 


// - 

// Function: Converter() 

// Return Value: None 
// Parameters: None 

// Description: Default constructor 

// - 

Converter::Converter(int deviceNo, unsigned int rowNo) 

: numberOfChannels(9), // to scan 

timeOut (180), // time out value 

status(0), // initial status 

sampleT_Base(-3), // board resolution,will be overwritten by DA O Rate 
scanT_Base(-3), // 20 KHz 

sampleinterval(100), // will be overwritten by DAQ_Rate 

scaninterval(0), //no time interval between conversions 

dBEnable(l), // enable double bufferring 

halfReady(O), // check for half ready 
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daqStop(O), 
rate (20000), 
inputMode(1), 
inputRange{0) 
polarity(0), 
drive(1), 
gain(2), 
ready (false) 
{ 


// check if DAQ stopped 
// DAQ rate 

// 0 - DIFF, 1 - RSE, 2 - NRSE 
// (ignored) 

// 0 - - 10/+10 1 - 0/+10 
// drive AI sense to onboard ground 
// channels have the same gain 
// flag for half ready 


if ( (rowNo >0) ScSc (deviceNo > 0)){ 

cerr«" Please wait while calibrating the converter 
# ” «deviceNo«endl ; 

deviceNumber = deviceNo; // device number 

// units used to express rate 0 ^ pts/sec ; 1 - sec/pts 

units=0; 

// the size of the double buffer 
bufferSize = rowNo * numberOfChannels * 2; 


// points to transfer to transfer buffer 
pointsTransfer = bufferSize / 2; 

// channels to scan 

channelVector = new short [numberOfChannels] ; 
assert (channelVector != 0) ; 

// gains per channel 

gainVector = new short [numberOfChannels]; 
assert (gainVector != 0); 

// double buffer, cirricular buffer 
doubleBuffer = new short [bufferSize]; 
assert (doubleBuffer != 0); 

for (int i=0;i<numberOfChannels;i++){ 
gainVector[i]=1; 

} 

switch (polarity) { 
case 0: 

maxRange = 32767; 

break; 
case 1: 

maxRange = 32767 *2+1; 

break; 

default: 

cerr«''ERROR : Unknown polarity\n" ; 

break; 

} 

calibrate(); 
initConverter(); 
printInitialValues(); 

}else{ 

cerr « "Converter can not initialized, " 

« "buffer row no must be greater than 0"«endl; 

} 

}//end Constructor 
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II - 

/ / Function: --Converter {) 

// Return Value: None 

// Parameters: None 

// Description: Destructor 

// - 

Converter::-Converter() 

{ 

stopO ; 

delete [] doubleBuffer; 
delete [] channelVector; 
delete [] gainVector; 
cout «”Converter deleted"«endl ; 
}// end destructor 


// - 

// Function: initConverter() 

// Return Value: None 
// Parameters: None 

// Description: Initialize the converter 

// - 

void Converter::initConverter() 

{ 

// init channels with physical numbers on the board 

channelVector[0]=0; 

channelVector[1]=1; 

channelVector[2]=2; 

channelVector[3]=3; 

channelVector[4]=4; 

channelVector[5]=5; 

channelVector[6]=6; 

channelVector[7]=7; 

channelVector[8]=8 ; 

// init gains for channels 
for (int i=0; i<nuinberOfChannels; i++) { 
gainVector[i]=gain; 

} 

// Configure converter to software triggered and to 
// use on board clock 

status = DAO Config (deviceNumber , 0, 0); 
if (status == 0){ 

cerr «"Converter configured"«endl; 

} else { 

cerr «"Error : Configure\n" ; 

} 

// set AI values, they could be changed by . 

// the config utility software 

status = AI_Configure (deviceNumber, -1, inputMode, inputRange, 

polarity, drive); 

if (status == 0) { 

cerr «’’AI configured”<<endl; 
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} else { 

cerr «"Error : Al^configure\n"; 

} 

// Configure the time out 

status = Timeout_Conf ig (deviceNumber, timeOut) ; 
if (status == 0){ 

cerr «”time out set"«endl; 

} else { 

cerr «”Error : Time out\n"; 

} 

// Enable double buffering operations 

status = DAQ_DB_Config (deviceNumber, dBEnable) ; 

if (status == 0){ 

cerr «"AD Converter initialized"«endl ; 

} else { 

cerr «status« " Problem with initialization\n" ; 

} 

cout «"Converter : InitializedXn"; 
return ; 

}//end initConverter() 


// 


// 


Function: 

calibrate() 

Return Value: 

None 

Parameters: 

None 

Description: 

Calibrates the converter 


void Converter: 

{ 


: calibrate() 


// calibrates the converter 

status = Calibrate_E_Series (deviceNumber, ND_SELF_CALIBRATE, 

ND_USER_EEPROM_AREA, 0.0); 

if (status != 0){ 

cout «"Error Calibrate\n" ; 
cout «status; 

} else{ 

cout «"\nConverter : AD Converter calibrated"; 

} 


// calculates and sets the rate 

status = DAQ_Rate (rate, units, &:sampleT_Base, &samplelnterval) ; 
if (status 1= 0){ 

cerr «"Error DAQ_Rate\n"; 
cerr «status; 

}else { 

cerr «"\nRate calculated \n"; 

} 

status =0; 
return ; 

}//end calibrate 0 
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II - 

// Function: printInitialValues() 

// Return Value: None 
// Parameters: None 

// Description: Prints the initial values 

// - 

void Converter:iprintInitialValues() 

{ 

cerr«"Channel * s gain is : "«gain«endl; 

switch (inputMode){ 
case 0: 

cerr<<”Mode : Differential\n"; 

break; 
case 1: 

cerr«”Mode : Referenced Single Ended\n"; 

break; 
case 2: 

cerr«"Mode : Non Referenced Single Ended\n" 

break; 
default: 

cerr« ” Unknown Mode \ n ” ; 

break; 

} 

switch (polarity){ 
case 0: 

cerr «"Polarity : Bipolar -10/+10\n"; 

break; 
case 1: 

cerr«"Polarity : Unipolar 0/+10\n"; 

break; 
default: 

cerr«”Unknown polarityXn”; 

break; 

} 

cerr «"Device nxxmber : "«deviceNuinber 

«"\nNumber of Channels : "«nuinberOfChannels 

«.. \nTransfer buffer depth : "« (bufferSize/(2* numberOfChannels) ) 
«”\nDouble buffer depth : " «(bufferSize/numberOfChannels) 
«"\nDouble buffer size : "« bufferSize 

«”\nSample Time Base : "«sampleT_Base 

«"\nSample Interval : ''«samplelnterval 

«”\nSampling rate : "«rate/numberOfChannels 

«”\nTime out : "<<timeOut<<endl; 

return; 

} // end printInitialValues() 
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// - 

// Function: getConfigData() 

// Return Value: None 

/ / Parameters: None 

// Description: Pass the values to the sampler 

// - 

void Converter:rgetConfigData(unsigned short Grange, 

unsigned short &myGain, int &halfDepth,int 
&block, double & deltaTime) 

{ 

double tBase = 0.0,nano = 1/1000000000.0, micro = 1/1000000.0; 
range = maxRange; 
myGain = gain; 

halfDepth = (bufferSize / (numberOfChannels*2)); 
block = numberOfChannels; 
if (sampleT__Base == -3){ 
tBase = 50 * nano; 

}else if (sampleT_Base == -1){ 
tBase =200 * nano; 

}else if (sampleT_Base == 1){ 
tBase = micro; 

}else if (sampleT_Base == 2){ 
tBase = 10 * micro; 

}else if (sampleT_Base == 3){ 
tBase =100 * micro; 

}else if (sampleT_Base == 4){ 
tBase = 0.001; 

}else if (sampleT_Base == 5){ 
tBase = 0.01; 

}else { 

cerr « "ERROR Converter = Unknown sample time base\n"; 
tBase = 0.0; 

}// deltaTime = 9 * halfDepth * tBase; 
deltaTime = 9 * halfDepth / rate; 
cerr«"Update rate : "« deltaTime«" seconds\n"; 
return; 

} // end getConfigData() 

// - 

// Fimction: start () 

// Return Value: None 
// Parameters: None 

// Description: Starts the data acquisition process 

// - 

void Converter::start() 

{ 

// set the scanning values 

status = SCAN__Setup (deviceNumber, numberOfChannels , 

channelVector, gainVector) ; 

if (status != 0){ 

cerr «status«" Error SCAN_Setup\n" ; 

} 

// start the scanning process 

status = SCAN_Start (deviceNumber , doubleBuffer, bufferSize, 
sampleT_Base, sampleinterval, scanT_Base, scaninterval); 
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if (status!=0){ 

cout «status<<” Error SCAN_Start\n''; 

} 

status = 0; 

cerr «"\nConverter : started\n”; 
return ; 

}//end start 0 


// - 

// Function: stopO 

// Return Value: None 
// Parameters: None 

// Description: Stops the converter 

// - 

void Converter::stop 0 

{ 

status = DAQ_Clear (deviceNijmber) ; 
status = DAQ_DB_Config(deviceNumber , 0); 
status = Timeout^Config(deviceNumber, -1); 
status = 0; 

cout <<”Converter : Stoped and cleared"«endl; 
return ; 

}//end StopO 

// - 

// Function: setEvents() 

// Return Value: None 
// Parameters: None 

// Description: Sets the events for converter 

// - 

void Converter::setEvents() 

{ //no events set 

return ; 

}//end setEvents0 

// - 

// Function: isHalfReady() 

// Return Value: true if half ready 

// Parameters: None 

// Description: First step to receive data from A/D converter 

// - 

bool Converter::isHalfReady() 

{ 

ready = false; 

// check status 
halfReady =0; 

while (!((halfReady == 1) && (status == 0))){ 

// check for half ready 

status = DAQ_DB_HalfReady (deviceNumber, SchalfReady, &daqStop) ; 
if (status != 0) { 

cerr«"Half ready error "«endl; 
restart (); 

} 

} 
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ready = true; 
return ready; 
}//end isHalfready() 


// - 

// Function: transferData() 

// Return Value: None 
// Parameters: None 

// Description: Transfers the half buffer to transfer buffer 

// - 

void Converter::transferData(short * trBuf) 

{ 

// transfer data 

status = DAQ__DB_Transfer (deviceNumber, trBuf, 

ScpointsTransfer, ScdaqStop) ; 

if (status != 0){ 

cerr«"Transfer error"«endl; 
restart(); 

} 

return ; 

}//end transferData0 


// - 

// Function: restart() 

// Return Value: None 
// Parameters: None 

// Description: Restarts the converter 

// -- 

void Converter::restart() 

{ 

cerr«’' RESTARTING Converter error status : "«status«endl; 
status = DAQ_Clear(deviceNumber); 
start(); 
return; 

} // end restart 0 


// - 

// Function: (friend) operator«() 

// Return Value; ostream object 

// Parameters: output - ostream object 

// C ~ Converter to print 

// Description: Prints the Converter information 

// - 

ostream &operator«(ostream &output, const Converter &C) 

{ 

output «" \nConverter : depth= "«C .bufferSize 

«endl ; 

return output; 

} / / end operator« 

//end of file Converter.cpp 
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G 


SAMPLER.H 


//**★*********★******★★★*★*****★*★*★*★***★*★*★★*★*★★★★*★*★★*★**★★**** 

//***********★**********★★*★*★★★**•★★★*****★★*★★★★★*★★★★****★*****★*★★ 

// 

// File : Sampler.h 
11 Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Sampler 

// - Receives data from A/D converter 

// - Averages, scales and converts this data 

// - Updates measurements with these data 

// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption ; None 
// 

// Warnings during compliation : None 
//******************************************************************* 
//**★★*★★★★★*★*★★★*★★★*★*****★*★★*★★*★*****★*****★*★★★★****★★**★***** 

#ifndef _SAMPLER_H_ 

#define _SAMPLER_H_ 

#include <iostream.h> 

#include <iomanip.h> 

#include "converter.h" 

#define M_PI 3.14159265358979323846 

#define GRAVITY 32.2185 //feet per second 


class Sampler{ 

// overloaded operator« 

friend ostream &operator«(ostream &,const Sampler &); 
public: 

// default constructor 
Sampler (Converter *); 

// destructor 
-Sampler(); 
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// finds zero level voltages for calibration 
void findZeroVoltages(bool); 

// get data from AD converter 
void getData(double double &); 

private: 


//AD converter 
Converter * myAD; 


int bufferDepth, 

// 

transfer buffer depth 

blockSize, 

// 

block, row size 

bufferSize, 

// 

transfer buffer size 

count; 

// 

number of readings 


unsigned short maxReading, // A/D converter max reading 

adGain; // A/D converter channel gain 

// variables used for conversions 

double angXConversion ,angZConversion ,angYConversion , 
accXConversion,accYConversion,accZConversion, 
magXConversion,magYConversion,magZConversion, 
angMultiplier,accMultiplier,magMultiplier, 
sampleTime, // sample time 

// zero level voltage values 
angXV, angYV, angZV, accXV, 
accYV,accZV, magXV, magYV, magZV, 

// scalar numbers for calibration 

angXScalar, angYScalar, angZScalar, accXScalar, accYScalar, 
accZScalar, magXScalar, magYScalar, magZScalar, 

// voltage range for converter 
VO It Ranger- 

short * transferBuffer; // transfer buffer 

// reads transfer buffer, updates readings 
void readAndUpdate(double *); 

// prints the transfer buffer 
void printTransferBuffer(); 

// converts readings 

double convert(double , const int); 

// helper function of convert() 

double convertHelper (double , double , double); 


}; 


tendif 

// end of file Sampler.h 
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H. SAMPLER.CPP 


//*★★****★*****************★★*★*★★*★*******★★*★★★★★**★**★★★**★★★★*★★★ 

//*★★★★★*★★★★**★★★★★★*★★***★★★★★★***★★★*★*★*★*★★★*•*★★*★★★***★***★**** 

// 

//File : Sampler.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Sampler 
// - Receives data from A/D converter 
// - Averages, scales and converts this data 
// - Updates measurements with these data 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 

//******************************************************************* 

//***★★*★★**★*★*★★★**★★★***★****★★★*★★★*★★*★★★★★★*★****★***★★*★★★**** 

tinclude <assert.h> 

#include <iomanip.h> 

#include <fstream.h> 

#include <sys/timeb.h> 

#include <stdio.h> 

#include <stdlib.h> 

#include <time.h> 

#include "sampler.h" 

#include "util.h" 

#include ”converter.h" 


// - 

// Function: Sampler{) 

// Return Value: None 
// Parameters: None 

// Description: Default constructor 

// - 

Sampler::Sampler (Converter * con) 
:myAD(con), // ray AD Converter 

count(O), // nuraber of samples 

maxReading(0), ft max converter reading 

adGain(O), // channel gain 

voltRange(10.0), // converter volt range 
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angXV{2.573),angYV(1.65),angZV(2.21), // assign zero voltage levels 
accXV(2.515),accYV(2.45),accZV(2.5), // also will be calculated 

magXV(2.45) ,inagYV{2.51) ,inagZV(2.509) , // by f indZeroVoltages () 

angXScalar(1.0),angYScalar(1.0),angZScalar(1.0), // scalar numbers 
accXScalar(2.99),accYScalar(2.0),accZScalar(1.0), // for calibration 
magXScalar(1.5), inagYScalar(1.8),magZScalar(1.8) 

// 2.99 2.99 2.99 2.0 2.0 1.415 4.0 4.0 4.0 tested scalars 

{ 

inyAD->getConfigData(inaxReading, adGain, bufferDepth, blocksize, 
sainpleTime) ; 

bufferSize = bufferDepth * blocksize; 
transferBuffer = new short [bufferSize]; 
assert (transferBuffer != 0); 
for (int k=0;k<bufferSize;k++){ 
transferBuffer[k] = 0; 

} 

angMultiplier = (90.0*(M_PI/180.0)) ; 
accMultiplier = (4 * GRAVITY); 
magMultiplier =2.0 ; 

}//end Constructor 


// 

// 

// 

// 

// 

// 


Function: 

-Sampler() 

Return Value: 

None 

Parameters: 

None 

Description: 

Destructor 


Sampler::-Sampler() 

{ 

cerr«" deleting sampler ”«endl; 
delete [] transferBuffer; 

}// end destructor 


// - 

// Function: 

// Return Value: 
// Parameters: 

// Description: 
// 

// - 


findZeroVoltages() 

None 

selfCalibrate - bool - to make self calibration 
Finds the zero level voltages, gets data from 
converter as many as repeat number 


void Sampler::findZeroVoltages(bool selfCalibrate) 

{ 

if (selfCalibrate){ 

const int REPEAT = 5; 

double values[9]={0.0}, zeros[REPEAT][6], total=0.0; 
long sum = 0; 


for (int r=0;r<REPEAT;r++){ 


// check if half buffer is ready 
while (!myAD->isHalfReady()){;} 
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} 


//call transfer function of AD 
myAD->transferData(transferBuffer); 

// find the averaged values 
for (int i=0;i<blockSize;i++){ 

for (int j=0;j<bufferSize; j+=blockSize){ 
sum += transferBuffer[j+i]; 

} 

values[i] = (sum/(bufferDepth*!.0)); 
sum = 0; 

} 

// fill zero level voltages matrix 
for (int t=0;t<6;t++){ 

zeros[r][t]= voltRange * values[t] / 
(maxReading * adGain); 

} 

zeros[r][5] *= (accMultiplier / 

(accMultiplier + GRAVITY)); 


// find averaged zero level voltages 
for (int k=0;k<6;k++){ 

for (int rr=0;rr<REPEAT;rr++){ 
total += zeros[rr][k]; 

} 

zeros[0][k] = total/REPEAT; 
total=0.0; 

} 

// assign zero level voltages 

angXV = zeros[0][0]; 

angYV = zeros[0][1]; 

angZV = zeros[0][2]; 

accXV = zeros[0][3]; 

accYV = zeros[0][4]; 

accZV = zeros[0][5]; 

/* 

for (int tt=0;tt<6;tt++){ 

cerr«" "«zeros [0] [tt] ; 

} 

*/ 

cerr«"Sampler calculated zero level voltages, by sampling" 
<<REPEAT<<" times\n\n"; 

} else { // use hard coded values 

cerr«"\nSampler is using hard coded values to calibrate\n"; 

} 

// assign conversion values with zero level voltages 
angXConversion = maxReading * angXV * adGain / voltRange;// 2.573 

angYConversion = maxReading * angYV * adGain / voltRange; // 1.65 

angZConversion = maxReading * angZV * adGain / voltRange; // 2.21 

accXConversion = maxReading * accXV * adGain / voltRange; // 2.51 
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accYConversion = maxReading * accYV * adGain / voltRange; // 2.46 

accZConversion = maxReading * accZV * adGain / voltRange; // 2.48 

magXConversion = maxReading * magXV * adGain / voltRange; 

magYConversion = maxReading * magYV * adGain / voltRange; 

magZConversion = maxReading * magZV * adGain / voltRange; 

return; 

} // end findZeroVoltages() 

// - 

// Function: getDataO 

// Return Value: None 

// Parameters: double m[] - measurements array from Filter 

// double & - delta t 

//Description: Communicates with converter, receives data into 

// transfer buffer, averages, scales, converts and 

// updates measurements 

// - 

void Sampler::getData (double * m , double & delta_T) 

{ 

delta_T = sampleTime; 

// errors will be taken care of by Converter 
while (!myAD->isHalfReady()){;} 

//call transfer function of AD 
myAD->transferData(transferBuffer); 

//printTransferBuffer(); 
readAndUpdate(m); 
return; 

}//end getDataO 


// - 

// Function: readAndUpdate() 

// Return Value: None 

// Parameters: double ma[] - same array with the getDataO 

// Description: Reads transfer buffer, averages data and updates 

// measurements 

If - 

void Sampler::readAndUpdate(double * ma) 

{ 

long sum =0; 

for (int i=0;i<blockSize;i++){ 

for (int j=0;j<bufferSize; j+=blockSize){ 
sum += transferBuffer[j+i]; 

} 

ma[i] = convert((sum/(bufferDepth*!.0)) , i); 
sum = 0; 

} 

return; 

}// end readAndUpdate() 
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// 

// 

// 

// 

// 

// 

// 

// 


Function: 
Return Value: 

convert() 

converted value - double 


Parameters: 

averaged - double - averaged value 
who -int - related reading no 

Description: 

Generic function, converts 
using related numbers 

the averaged value by 


double Sampler::convert(double-averaged , const int who) 

/ 

double result = 0.0; 
switch (who) { 

case 0: // body rate p 

result = convertHelper(averaged, angXConversion, 
angMultiplier * angXScalar); 

break; 

case 1: // body rate q 

result = convertHelper(averaged, angYConversion, 
angMultiplier * angYScalar); 

break; 

case 2: // body rate r 

result = -convertHelper(averaged, angZConversion, 
angMultiplier * angZScalar); 

break; 

case 3: // accelerometer hi 

result = -convertHelper(averaged, accXConversion , 
accMultiplier * accXScalar); 

break; 

case 4: // accelerometer h2 

result = -convertHelper(averaged, accYConversion , 
accMultiplier * accYScalar); 

break; 

case 5: // accelerometer h3 

result = -convertHelper(averaged , accZConversion , 
accMultiplier * accZScalar); 

break; 

case 6: // magnetometer bl 

result = -convertHelper(averaged , magXConversion, 
magMultiplier * magXScalar); 

break; 

case 7: // magnetometer b2 

result = -convertHelper(averaged , magYConversion, 
magMultiplier * magYScalar); 

break; 

case 8: // magnetometer b3 

result = -convertHelper(averaged , magZConversion, 
magMultiplier * magZScalar); 

break; 

default: 

cout «”Error in Sampler: Check your data block size\n"; 
break; 

} 

return result; 

}// end convert() 
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// - 

// Function: 

// Return Value: 

// Parameters: 


// Description: 
// 


convertHelper{) 
converted value 

theNumber - double - averaged value 
adConversion - double ~ related conversion number 
mult - double - related multiplier contains scalar 
Generic function, converts the averaged value by 
using related numbers 


double Sampler:'.convertHelper (double theNumber , double adConversion, 

double mult) 


return (((theNumber - adConversion) / adConversion) * mult); 
}// end convertHelper() 


// - 

// Function: printTransferBuffer() 

// Return Value: None 
// Parameters: None 

// Description: Prints the transfer buffer 

// - 

void Sampler::printTransferBuffer() 

{ 

for (int i=0;i<bufferDepth;i++){ 

for (int j=0;j<blockSize;j++){ 

cout«setprecision (15) «transferBuffer [ j + (i*blockSize) ]«" " 

} 

cout «endl; 

} 

return; 

}//end printTransferBuffer() 


// - 

// Function: (friend) operator«() 

// Return Value: ostream object 
// Parameters: output - ostream object 

// s - sampler to print 

// Description: Prints the sampler information 

// - 

ostream &operator« (ostream &output, const Sampler Scs) 

{ 

output «"\nsampler : depth= "«s.bufferDepth 

«" block size= "«s.blockSize 
«" transfer buffer size= "«s.bufferSize 
«” current counter = " «s. count«endl; 

return output; 

}// end operator« 

//end of file Sampler.cpp 
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I 


MATRIX.H 


//★★★★*★★*★**★********★*★★★*★★**★★★★★★*★★★★★★★*★***★***★★★*******★★** 
// 

// File ; Matrix.h 
// Author : ildeniz DUMAN 

// Project Name : Matrix Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Header file for Matrix.cpp 
// - Executes matrix operations 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 
//****★★*****★★★★★★★★**★★★*★★★★***★**★★★★★★★*★★★★★*★*★*★★★***★★**★*★* 
//******★★**★*★*★★★★★***★*★★****★★★*★*★★*******★★**★*★★★*★★*★★*★★★*** 

#ifndef MATRIX H_ 

#define _MATRIX_H_ 

#include <iostream.h> 

#include <iomanip.h> 

#include "quaternion.h” 


class Matrix{ 

// overloaded operator« 

friend ostream &operator«(ostream &,const Matrix &); 
public: 

// default constructor 

Matrix (char * mname="Matrix”,int mrow = 4,int mcol = 4); 

//conversition constructor from a two dimensional double array 
Matrix (char * mname,int arrayRow, int arrayCol,double **); 

// destructor 
-Matrix(); 

// copy constructor 
Matrix (const Matrix &); 
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// matrix invertion 
Matrix invert{); 

// transpose 

void transpose (Matrix &) const; 

// Matrix product 

Matrix operator*(const Matrix &) const; 

// Matrix assignment 

Matrix &operator=(const Matrix &); 

// Matrix product and assignment 
Matrix S:operator*= (const Matrix Sc); 

II creates a unit matrix 
Matrix unitMatrix(int); 

// inserta a quaternion in a row 

void insertRowQuaternion(Quaternion & a, Quaternion & b, int) 
// inserta a quaternion in a col 

void insertColQuaternion(Quaternion & d, Quaternion & c, int) 

// return row no 

int getRowO {return row;} 

// return col no 

int getCol0{return col;} 

// returns an element from the matrix 

double getElement(int i, int j){ return matrix[i][j];} 
private: 

// the name of the Matrix 
char * name; 

// the elements of a Matrix 
double * * matrix; 

//row and column 
int row, col; 


}; 


#endif 

// end of file Matrix.h 
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J 


MATRIX.CPP 


//*★*****★**★*****★★**★★*★*******★***★*****★*★***★***★*★*★*★★****★★★* 
//*★*****★****★******★*★***★*****★****★***★********★*★*★*★★**★*****★* 
// 

// File : Matrix.cpp 
// Author : ildeniz DUMAN 

// Project Name : Matrix Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Matrix 
// - Executes matrix operations 
// 

// Inputs : None 

// 

// Outputs : None 

// 

// Process : None 

// 

// Assumption : None 

// 

// Warnings during compliation : None 

//*★★*★★★*★★***★★**★*★★★★*★*★*★★*★**★**★★*★*****★**★★★*******★*★★★*★* 

//******************************************************************* 


#include 
#include 
#include 
#include 
#include 


<math.h> 
<string.h> 
<assert.h> 
"Matrix.h" 

•' guaternion.h" 


//- 

— 


// 

Function: 

Matrix() 

// 

Return Value: 

None 

// 

Parameters: 

mname - name o f the matrix 

// 


mrow - row no 

// 


mcol - col no 

// 

/ /. 

Description: 

Default constructor 


Matrix::Matrix (char* mname,int mrow, int mcol) 
: row (mrow) , col (mcol) 

{ 

int length = strlen(mname); 
name = new char[length+1]; 
assert (name != 0); 
s t r cpy (name, mname) ; 

matrix = new double *[row]; 
assert (matrix !-0); 
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for (int X = 0; x<row; x++){ 

inatrix[x] = new double [col] ; 
assert (matrix[x] 1=0); 

} 

for (int i = 0;i<row;i++){ 

for (int j = 0;j<col;j++){ 
matrix[i][j] = 0.0; 

} 


} 

}//end Constructor 


// 


// 


Function: 

-Matrix() 

Return Value: 

None 

Parameters: 

None 

Description: 

Destructor 


Matrix::-Matrix() 

{ 


delete [] name; 
for (int x=0;x<row;x++){ 
delete matrix[x]; 

} 

delete [] matrix; 

}//end destructor 


// 

// 

// 

// 

// 

// 


Function: 

Matrix(Matrix &) 

Return Value: 

None 

Parameters: 

MAT - Matrix to copy 

Description: 

Copy constructor 


Matrix: :Matrix (const Matrix &MAT) 

{ 

int length = strlen(MAT.name); 


name = new char[length+1]; 

assert(name != 0); 

strcpy (name,MAT.name) ; 

matrix = new double * [MAT. row]; 

assert (matrix !=0); 

for (int X = 0; x<MAT.row; x++){ 

matrix[x] = new double [MAT.col]; 
assert (matrix[x] !=0); 

} 

row=MAT. row; 
col=MAT.col; 

for (int i = 0;i<MAT.row;i++){ 

for (int j = 0;j<MAT.col;j++){ 

matrix[i][j] = MAT.matrix[i][j]; 

} 

} 

}// end copy constructor 
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// - 

// Function: Matrix() 

// Return Value: None 

// Parameters: mname - name of the matrix 

// arow - row no 

// acol “ col no 

// double [][] - a - two dimesional double array 

// Description: Constructs a matrix from a two dimensional array 

// - 

Matrix::Matrix (char * mname , int arow, int acol,double ** a) 

{ 

int length = strlen(mname) ; 

name = new char[length+1]; 

assert (name != 0); 

strcpy (name,mname) ; 

matrix = new double *[arow]; 

assert (matrix 1=0); 

for (int X = 0; x<arow; x++){ 

matrix[x] = new double [acol]; 
assert (matrix[x] 1=0); 

} 

row = arow; 
col = acol; 

for (int i = 0;i<row;i++){ 

for (int j = 0;j<col;j++){ 

matrix[i][j] = a[i][j]; 

} 

} 

}// end Matrix0 


// - 

// Function: operator*() 

// Return Value: Matrix 

// Parameters: MAT - Matrix & - Matrix to multiply 

// Description: Calculates the Matrix product 

// - 

Matrix Matrix::operator*(const Matrix &MAT) const 

{ 

Matrix dest("Product", row , MAT.col); 

double sum = O.Of; 

for (int i=0;i<row;i++){ 

for (int j=0;j<MAT.col;j++){ 

for (int k=0;k<MAT.row;k++){ 

sum += matrix[i] [k] * MAT.matrix[k][j] ; 

} 

dest.matrix[i][j]=sum; 
sum = 0.0; 

} 

} 

return(dest); 

}//end operator* 
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11 - 

// Function: operator=() 

// Return Value: Matrix & 

// Parameters: MAT - Matrix & 

// Description: Assigns the MAT to current object 

If - 

Matrix & Matrix::operator=(const Matrix &MAT) 

{ 

/*if (&MAT != this){ 
delete [] name; 

int length = strlen(MAT.name); 
name = new char[length+1]; 
assert(name 1= 0); 
s t rcpy (name, MAT. name) ; 

} 

*/ 

//I let self assingment 
if ((rowl=MAT.row) || (col != MAT.col)){ 
cout «"Error in matrix assignment 
} else { 

delete [] name; 

int length = strlen(MAT.name); 
name = new char[1ength+1]; 
assert(name != 0); 
strcpy(name,MAT.name) ; 

for (int i = 0;i<MAT.row;i++){ 

for (int j = 0;j<MAT.col;j++){ 

matrix[i][j] = MAT.matrix[i][j]; 

} 

} 

} 

return (*this); 

}//end operator= 

// - 

// Function: unitMatrixO 

// Return Value: Matrix 

// Parameters: rowOrCol -- int - square matrix index 

// Description: Creates a unit matrix 

// --- 

Matrix Matrix::unitMatrix (int rowOrCol) 

{ 

Matrix Unit("unit", rowOrCol , rowOrCol); 
for (int i = 0 ; i<Unit.row ; i++){ 

Unit.matrix[i][i] = 1.0; 

} 

return (Unit); 

}// end unitMatrix() 
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II - 

// Function: invert() 

// Return Value: Matrix 
// Parameters: None 

// Description: Calculates the matrix inversion 

// - 

Matrix Matrix::invert() 

{ 

double multiplier=0.0 , divider =0.0; 

Matrix myUnit=myUnit .unitMatrix(row) ; 

// square matrix check 
if (row == col){ 

//inverting the matrix 
for (int j=0; j<col;j++){ 

for (int i=0; i<row;i++){ 
if (i != j ){ 

multiplier = -matrix[i][j]/matrix[j][j]; 
for (int k=0;k<col;k++){ 

matrix[i][k] += (multiplier * 
matrix [j][k]); 

myUnit.matrix[i][k] += (multiplier 
* myUnit.matrix[j][k]); 


// final division to make our matrix a unit matrix 
for (int i = 0 ; i<row ; i++){ 
divider = matrix[i][i]; 
if (divider != 0.0){ 

matrix [i][i] /= matrix [i][i]; 
for (int k=0;k<myUnit.row;k++){ 

myUnit.matrix [i][k] /= divider; 

} 

} 

} 

} else { 

cout « "Error : Matrix must be a square matrix "«endl; 

} 

return (myUnit); 

}// end unitMatrix0 
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// - 

// Function: operator*={) 

// Return Value: Matrix 

// Parameters: MAT - Matrix & 

// Description: Calculates the product and assigns the result to 

// current object 

// - 

Matrix 8c Matrix: : operator*= (const Matrix &MAT) 

{ 

*this = *this * MAT; 
return (*this); 

}// end operator*= 


// - 

// Function: transpose{) 

// Return Value: None 

// Parameters: tr - Matrix 

// Description: Finds the transpose of a matrix 

// - 

void Matrix: : transpose (Matrix & tr) const 

{ 

if ((row == tr.col) && (col == tr.row)){ 
for (int i=0;i<row;i++){ 

for (int j=0;j<col;j++){ 

tr.matrix[j][i] = matrix[i][j]; 

} 

} 

} 

return; 

}// end transpose() 


// - 

// Function: insertRowQuaternion() 

// Return Value: None 

// Parameters: Q1 , Q2 - Quaternions 

// rowNo - int - row no to insert 

// Description: Takes two quaternions and inserts them in a row by 

// ignoring the w values of the quaternions 

// - 

void Matrix::insertRowQuaternion (Quaternion & Ql, Quaternion & Q2, 

int rowNo) 

{ 

if ((row >= rowNo) && (col==6)){ 

matrix [rowNo] [0] = Ql.getXO; 
matrix [rowNo] [1] = Ql.getYO; 
matrix [rowNo] [2] = Ql.getZO; 
matrix [rowNo] [3] = Q2.getX(); 
matrix[rowNo][4] = Q2.getY(); 
matrix[rowNo][5] = Q2.getZ(); 

} 

return; 

}// end insertRowQuaternion() 
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// - 

// Function: 

Return Value: 
Parameters: 


// 

// 

// 

// 

// 

// 


Description: 


insertColQuaternion() 

None 

Q1 , Q2 - Quaternions 

colNo “ int - coloumn no to insert 

Takes two quaternions and inserts them in a coloumn 
by ignoring the w values of the quaternions 


void Matrix::insertColQuaternion ( Quaternion & Q1, Quaternion & Q2, 

int colNo) 


{ 


if ((col >= colNo) && 
matrix[0][colNo] 
matrix[1][colNo] 
matrix[2][colNo] 
matrix[3][colNo] 
matrix[4][colNo] 
matrix[5][colNo] 

} 

return; 

}//end insertColQuaternion() 


(row==6)){ 

= Ql.getXO ; 
= Q1.getY(); 
= Q1.getZ(); 
= Q2.getX(); 
= Q2.getY{); 
= Q2.getZ(); 


// 


// 

// 

// 

// 

// 


Function: 

(friend) operator« () 

Return Value: 

©stream object 

Parameters: 

output “ ostream object 
q - Matrix to print 

Description: 

Prints the Matrix in a form, should be written out 
of class 


©stream &operator« (©stream &:Output, const Matrix &:q) 

{ 

output «’ ['«q.name«']'«" "«q.row«’'x'*«q.col«endl; ; 


for (int k=0;k<q.row;k++){ 

for (int m=0;m<q.col;m++) { 

output «" ''«q.matrix[k] [m] ; 

} 

output «endl; 

} 

return output; 

} / / end operator« 

//end of file Matrix.cpp 
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K. QUATERNION.H 


//***★**★**★★*★******★★*★★★****★★********★★★★******★**★**★★*★★*★***** 

//****★**************★**************★***********★★***★*★************* 

// 

// File : Quaternion.h 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Header file for quaternion.cpp 
// - Executes Qauternion operations 
// 

// Inputs : None 
// 

// Outputs : None 
11 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 
//★***★*★★*★★*****★******★★★★**★**************★**★***★****★★*★★****★★ 
//★****★*******•★*********★**************★★★**★★*****★****★★**★★★**★★* 

#ifndef _QUATERNION^H_ 

#define _QUATERNION_H_ 

tinclude <iostream.h> 

#include <iomanip.h> 

#define M_PI 3.14159265358979323846 


class Quaternion{ 

// overloaded operator* 

friend Quaternion operator*(const double,Quaternion &); 

// overloaded operator« 

friend ostream &operator«(ostream &,const Quaternion &); 
public: 

// default constructor 

Quaternion (char * qname="Quaternion", double ww=0.0, 
double xx=0.0,double yy=0.0,double zz=0.0); 

// destructor 
-Quaternion(); 
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// copy constructor 
Quaternion (const Quaternion &); 

// get data members 
double getW(); 
double getX(); 
double getY(); 
double getZ(); 

// quaternion product 

Quaternion operator*(const'Quaternion &) const; 

// overwriting operator*() to make scalar multiplications 
Quaternion operator*(const double); 

// Quaternion addition 

Quaternion operator+(const Quaternion &) const; 

// Quaternion substraction 

Quaternion operator-(const Quaternion &) const; 

// quaternion assignment 

Quaternion &operator=(const Quaternion &); 

// quaternion product and assignment 
Quaternion &operator*=(const Quaternion &); 

// quaternion inverse (conjugate) 

Quaternion operator-() const; 

//sets the quaternion 

void setQuaternion(double , double , double , double); 

//converts to Euler Angles 
Quaternion toEulerAngles(); 

// rotate a quaternion about a 3D vector (w=0) 

Quaternion rotation(const Quaternion &); 

II converts to body coordinates 
Quaternion toBody(const Quaternion &); 

// dot product 

double dotProduct(const Quaternion &); 

// quaternion to axis angles 
Quaternion toAxisAngles(); 

// rotation angles(w=0) to quaternion 
Quaternion toQuaternion(); 

// normalizes a quaternion 
void normalize(); 
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// calculates partial derivative 

Quaternion partialDerivative (const Quaternion Sc, 

const Quaternion Sc, const Quaternion Sc) const; 


private: 

// the name of the quaternion 
char * name; 

// the elements of a quaternion 
double w,x,y,z; 


}; 


#endif 

// end of file Quaternion.h 
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L. QUATERNION.CPP 


//★★★**★★*★*★**★★★★★*★★***★*★★**★*★*****★★*★*★***********★★★**★*★★*** 

//*★*★*****★*★****★****★*★*★★**★*★★★★★★★★★******★★*************★*★**★ 

// 

// File : Quaternion.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for quaternion 
II- Executes Qauternion operations 
// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assumption : None 
// 

// Warnings during compliation : None 

//*★★*★********★★★*******★*★★★***★***★***★*******★★***************★★* 

Z/******************************************************************* 


#include 

#include 

#include 

#include 


<math.h> 
<string.h> 
<assert.h> 

"Quaternion.h" 


/ / ■ 

// 

Function: 

Quaternion() 

// 

Return Value: 

None 

// 

Parameters: 

None 

// 

Description: 

Default constructor 

//■ 

- 



Quaternion::Quaternion (char* qname,double ww,double xx,double yy, 

double zz) 

:w(ww) ,x(xx) ,y(yy) , z (zz) 

{ 

int length = strlen(qname); 
name = new char[length+1]; 
assert (name != 0); 
s t rcpy(name,qname); 

}//end Constructor 
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// - 

// Function: -Quaternion() 

// Return Value: None 

// Parameters: None 

// Description: Destructor 

// - 

Quaternion::-Quaternion() 

{ 

/ /cout «"deleting ”«*this«endl ; 

delete [] name; 

}//end destructor 


// - 

// Function: Quaternion(Quaternion &) 

// Return Value: None 

// Parameters: Quaternion 

// Description: Copy constructor 

// - 

Quaternion::Quaternion(const Quaternion &QUAT1) 

{ 

int length = strlen(QUATl.name); 

name = new char [1 ength+1] ; 
assert(name != 0); 
strcpy (name, QUATl. name) ; 
w = QUATl.w; 

X = QUATl.x; 
y = QUATl.y; 
z = QUATl.z; 

}// end copy constructor 


// --- 

// Function: setQuaternion() 

// Return Value: None 

// Parameters: double a,b,c,d - values to set 

// Description: Sets the quaternion with new values 

// - 

void Quaternion::setQuaternion (double a, double b, double c, double d) 

{ 

w=a; 

x=b; 

y=c; 

z=d; 

return; 

)// end setQuaternion() 


// get functions returns the data members 
double Quaternion::getW(){return w;} 
double Quaternion::getX(){return x;} 
double Quaternion::getY(){return y;} 
double Quaternion::getZ(){return z;} 
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II - 

// Function: operator*() 

// Return Value: Quaternion 

// Parameters: QUAT - Quaternion & *- First rotation 

// Description: Calculates the quaternion product 

// - 

Quaternion Quaternion: :operator* (const Quaternion &:QUAT) const 

{ 


Quaternion dest("Quaternion Product"); 


dest.w = 

QUAT.w 

* 

w 

- 

QUAT.X 

★ 

X 

- QUAT.y 

* 

y 

- 

QUAT.z 

* 

z 

dest.x = 

QUAT.W 

* 

X 

+ 

QUAT.x 

* 

w 

” QUAT.y 

★ 

z 

+ 

QUAT.z 

★ 

y 

dest.y = 

QUAT.w 

* 

y 

+ 

QUAT.y 

★ 

w 

- QUAT.z 

* 

X 

+ 

QUAT.X 

* 

z 

dest.z = 

QUAT.w 

* 

z 

+ 

QUAT.z 

★ 

w 

- QUAT.X 

★ 

y 


QUAT.y 

* 

X 


return(dest); 
}//end operator* 


// - 

// Function: operator*(double) overwriten 

// Return Value: Quaternion 

// Parameters: NUM - double - a scalar to multiply 

// Description: Calculates the scalar product, double must be on 

// the right 

// - 

Quaternion Quaternion::operator*(const double NUM) 

{ 

w=NUM*w; 
x=NUM*x; 
y=NUM*y; 
z=NUM*z; 
return (*this); 

}//end operator*(double) 


// - 

// Function: operator+ 

// Return Value: Quaternion 

// Parameters: QUAT - Quaternion - a quaternion to add 

// Description: Calculates the quaternion addition 

// - 

Quaternion Quaternion::operator+(const Quaternion &QUAT) const 

{ 

Quaternion add("addition"); 
add.w = w + QUAT.w; 
add.x = X + QUAT.x; 
add.y = y + QUAT.y; 
add .2 = z + QUAT.z; 

return (add); 

} // end operator+() 
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// - 

// Function: operator- 

// Return Value: Quaternion 

// Parameters: QUAT - Quaternion - a quaternion to substract 

// Description: Calculates the quaternion substraction 

// - 

Quaternion Quaternion::operator-(const Quaternion &QUAT) const 

{ 

Quaternion add("substraction"); 

add.w = w - QUAT.w; 

add.x = X - QUAT.x; 

add.y = y - QUAT.y; 

add.z = z - QUAT.z; 

return (add); 

}// end operator-() 


// - 

// Function: operator=() 

// Return Value: Quaternion & 

// Parameters: QUAT2 - Quaternion & 

// Description: Assigns the QUAT2 to current object 

// - 

Quaternion & Quaternion::operator=(const Quaternion &QUAT2) 

{ 

/*if (&QUAT2 1= this){ 
delete [] name; 

int length = strlen(QUAT2.name); 
name = new char [length+1] ; 
assert(name != 0); 
strcpy (name, QUAT2 . name) ; 

} 

I let self assingment */ 

W = QUAT2.W; 

X = QUAT2.X; 
y = QUAT2.y; 
z = QUAT2.Z; 

return (*this); 

}//end operator= 


// 


// 

// 

// 

// 

Quaternion & Quaternion::operator*: 

{ 


Function: 

operator*=() 

Return Value: 

Quaternion 

Parameters: 

QUAT3 - Quaternion & 

Description: 

Calculates the product and assigns the result to 
current object 


: (const Quaternion &QUAT3) 


*this = *this * QUAT3; 
return (*this); 

}// end operator*^ 
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// - 

// Function: operator-() 

// Return Value: Quaternion 
// Parameters: None 

// Description: Calculates the inverse (conjugate) quaternion 

// - 

Quaternion Quaternion::operator-() const 

{ 

Quaternion temp("Conjugate"); 

temp.w = w; 

temp .X = -X; 

temp.y = -y; 

temp .2 = -z; 

return (temp); 

}//end operator- 


- 

// Function: 

// Return Value: 
// Parameters: 

// Description: 
//- 


rotation() 

Quaternion 

QUAT4 - Quaternion & - vector to rotate around 
Calculates the rotation quaternion 


Quaternion Quaternion::rotation(const Quaternion &QUAT4) 

{ 

Quaternion temp("Rotation"); 

temp = *this * ( QUAT4 * (-(*this))); 

return (temp); 

}//end Rotation 0 


// 

// 

// 

// 

// 

// 

// 


Function: 

toBody() 


Return Value: 

Quaternion 


Parameters: 

QUAT4 - Quaternion & - 
coordinates 

vector to convert to body 

Description: 

Converts into the body 

coordinates 


Quaternion Quaternion::toBody(const Quaternion &QUAT) 

{ 

Quaternion temp("to Body"); 

temp = ((-(*this)) * (QUAT * (*this))); 

return (temp); 

}//end Rotation 0 
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// ----- 

// Function: dotProduct() 

// Return Value: double 
// Parameters: QUAT4 - Quaternion & 

// Description: Calculates the dot product of quaternions 

// - 

double Quaternion::dotProduct(const Quaternion &QUAT5) 

{ 

double dotproduct; 

dotproduct = (w * QUATS.w) + (x * QUATS.x) + (y * QUAT5.y) + 
(z * QUAT5.Z); 

return (dotproduct); 

} //end dotproduct0 


// - 

// Function: noimializeO 

// Return Value: None 
/ / Parameters: None 

// Description: Normalizes the quaternion 

f f - 

void Quaternion::normalize() 

{ 

double normal; 

normal = (x * x) + (y * y) + (z * z) + (w * w) ; 
X = X / sqrt(normal); 
y = y / sqrt(normal); 
z = z / sqrt(normal); 
w = w / sqrt (normal); 
return; 

}//end normalize 0 


// - 

// Function: toAxisAngles() 

// Return Value: Quaternion 

// Parameters: None 

// Description: Calculates the axis angles of quaternion, results 

// must be in glRotatefO 

// - 

Quaternion Quaternion::toAxisAngles() 

{ 

Quaternion axisAngle("To Axis Angle "); 

double scalarNumber,temp; 
temp = acos(w) * 2.0; 
scalarNumber = sin(temp / 2.0); 
axisAngle.X = x / scalarNumber; 

// aligning the axes 
axisAngle.y = -1 * z / scalarNumber; 
axisAngle.z = y / scalarNumber; 
axisAngle.w = (temp * (360 / 2.0)) / M_PI; 
return (axisAngle); 

}//end toAxisAngles() 
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// - 

// Function: toQuaternion() 

// Return Value: Quaternion 

// Parameters: None 

// Description: Calculates the quaternion value of three rotations 

// - 

Quaternion Quaternion::toQuaternion() 

{ 

//current object is a 3D vector with rotation angles at x,y,z 
double radx,radY,radZ,tempx,tempy,tempz; 

double cosx,cosy,cosz,sinx,siny,sinz,cosc,coss,sine,sins; 
Quaternion quat("Euler To Quaternion"); 

// convert angles to radians 

radX = (X * M_PI) / {360.0 / 2.0); 

radY = (y * M_PI) / (360.0 / 2.0); 

radZ = (z * M_PI) / (360.0 / 2.0); 

// half angles 
tempx = radX * 0.5; 
tempy = radY * 0.5; 
tempz = radZ * 0.5; 
cosx = cos(tempx); 
cosy = cos(tempy); 
cosz = cos(tempz); 
sinx = sin(tempx); 
siny = sin(tempy); 
sinz = sin(tempz); 

cosc = cosx * cosz; 

coss = cosx * sinz; 

sine = sinx * cosz; 

sins = sinx * sinz; 

quat.x = (cosy * sine) - (siny * coss); 

quat.y = (cosy * sins) + (siny * cosc); 

quat.z = (cosy * coss) - (siny * sine); 

quat.w = (cosy * cosc) + (siny * sins); 

quat.normalize() ; 
return(quat); 

}// end toQuaternion 

// - 

// Function: (friend) operator*(double,Quaternion &) overwriten 

// Return Value: Quaternion 

// Parameters: NUM - double - a scalar to multiply 

// Description: Calculates the scalar product 

// - 

Quaternion operator*(const double NUM,Quaternion & quat) 

{ 

quat. w=NUM*qua t. w; 
quat.x=NUM*quat.x; 
quat. y=NUM* quat. y ; 
quat.z=NUM*quat.z; 
return (quat); 

}//end operator*(double,Quaternion &) 
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// 

// 

// 

// 

// 

// 

// 


Function: 

toEulerAngles() 


Return Value: 

Quaternion 


Parameters: 

None 


Description: 

Converts quaternion into 

euler angles , this 


conversion is inherently 

ill-defined 


Quaternion Quaternion::toEulerAngles{) 

{ 

Quaternion euler("Quat to euler"); 

double sint,cost,sinv,cosv,sinf,cosf,ex,ey,ez; 


sint = (2*w*y)-(2*x*z); 
cost = sqrt(l~ pow(sint,2)); 
if (cost != 0.0){ 

sinv = {{2*y*z)+(2*w*x))/cost; 
cosv = (1“(2*x*x)-(2*y*y))/cost; 
sinf = (1-(2*x*x)-(2*y*y))/cost; 
cosf = (1-(2*y*y) -(2*z*z))/cost; 
}else { 

sinv = (2*w*x)-(2*y*z); 
cosv = 1-(2*x*x)-(2*z*z); 
sinf = 0.0; 
cosf = 1.0; 

} 

ex = atan2(sinv,cosv); 
ey = atan2(sint,cost); 

ez = atan2(sinf,cosf); //range -pi +pi 

ex *= 180.0/M_PI; 
ey *= 180.0/M_PI; 
ez *= 180.0/M_PI; 

euler.setQuaternion(0.0, ex,ey,ez); 
return euler; 

}//end toEulerAngles() 


//- 

// Function: 

// Return Value: 
// Parameters: 

// 

// 

// 


// Description: 
// 

//- 


partialDerivative() 

Quaternion 

M “ vector, as in the X matrix 
QHAT - q-hat 

DER - partial derivative of quaternion 
INVDER - inverse partial derivative of quaternion 
Calculates the partial derivatives for X Matrix, 
special to filter 


Quaternion Quaternion::partialDerivative(const Quaternion & QHAT, 

const Quaternion & DER,const Quaternion & INVDER) const 


{ 


return ((INVDER*((*this) * QHAT))+((-QHAT)*((*this)*DER))); 
} / / end partialDerivative() 
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If - 

// Function: (friend) operator<<() 

// Return Value: ostream object 

// Parameters: output - ostream object 

// q - quaternion to print 

// Description: Prints the quaternion in a form, should be written 

// out of class 

// - 

ostream &operator«(ostream ^output, const Quaternion &q) 

{ 

/* output «q.name«" w= “«q.w 


«" 

x= 

"«q. 

.X 

«" 

y= 

"«q- 

•y 

«" 

z = 

”«q. 

. z ; * / 


output << q.w <<" "<<q.x<<" "<<q.y<<" ''<<q.z<<'' " 
return output; 

}// end operator« 

//end of file Quaternion.cpp 
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M. UTIL.H 


//*****★★****★*★**★*****★***************★**★**★*****★***★**★★*★★**★** 

/y******************************************************************* 

// 

// File : Util.h 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Header file for Utilities 

// 

// Inputs : None 

// 

// Outputs : None 

// 

// Process : None 

// 

// Assumption : None 

// 

// Warnings during compliation : None 

//******************************************************************* 

yy******************************************************************* 

#ifndef _^UTIL_H_ 

#define _^UTIL_H_ 


#include <iostream.h> 
#include <fstream.h> 


// opens a file to read 

int openFile (ifstream & ,char * ); 

// opens a file to write 

int writeFile (ofstream & ,char * ); 

#endif 

// end of file Util.h 
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N. 


UTIL.CPP 


/y'*********************************************************'********** 

// 

// File : Util.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for Utilities 
// - Opens a file to write and read 
// 

// Inputs : None 

// 

// Outputs : None 

// 

// Process : None 

// 

// Assumption : None 

// 

// Warnings during compliation : None 

//******★***★*★********★★*★★*★**★*★★*★*★*★★★*****★★*★*★★★★★****★★**** 

//*★★★★★***★***★★**★***★★****★*★*★*★★*★***★★★★★*****************★*★** 


#include 
#include 
#include 
#include 


<iostream.h> 

<fstream.h> 

<iomanip.h> 

"util.h" 


// 

// 

// 

// 

// 

// 

// 


Function: 

openFile() 

Return Value: 

None 

Parameters: 

inDataFile - & ifstream - ifstream object 
filename - char * - file name to open 

Description: 

Opens a file to read 


int openFile (ifstream & inDataFile ,char * fileName) 

{ 


int error = 0; 


inDataFile.open (fileName , ios::in); 

cout «"Reading "«fileName«'* for configuration settingsXn"; 
if (!inDataFile){ 

cout «"File ’'«fileName«" not foundXn"; 
error = 1; 

} 

return error; 

}// end openFile0 
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// 


// 

11 
// 

// 

int writeFile(ofstream & outDataFile, char *fileName) 

{ 

int error = 0; 


Function: 

writeFile() 


Return Value: 

None 


Parameters: 

inDataFile - & ifstream - ifstream object 
filename - char * - file name to open 

Description: 

Opens a file 

to write 


outDataFile.open (fileName , ios::out); 
cout «fileName«" opened to write\n"; 

if (IoutDataFile){ 

cout «"File "«fileNaine«'' not foundXn"; 
error = 1; 


return error; 

}// end writeFile0 

//end of file util.cpp 


153 




O. TEST.CPP 


//★**★**★***★*★*★**★*★**★★★**★***★★*★*★★***★★★**★★★★*★*★**★★★***★★★★* 

// 

// File : test.cpp 
// Author : ildeniz DUMAN 

// Project Name : Quaternion Attitude Estimation Filter 
// Operating Environment : MS-Windows 95 
// Compiler : Visual C++ 5.0 
// Date : 01/09/99 
// 

// Description : 

// 

// - Source file for test 

// - Sample driver file for the filter 

// 

// Inputs : None 
// 

// Outputs : None 
// 

// Process : None 
// 

// Assiamption : None 
// 

// Warnings during compliation : None 
// 

// Note : nidaq32.1ib, nidaq.h and nidaqcns.h must be added to the 
// project file 

//***★*★*★*★**★*★★★**★*★★*★★**★★★★**★★★★*★★*★★**★★**********★*****★★* 

//****★********★★**★★★*★**★*****★*★★*★★*★★****★**★*★★**★***★*******★* 


#include 
#include 
#include 
#include 


<math.h> 

"qaef.h” 

"quaternion.h" 

"util.h” 


int main () { 

// file name to log data 

char * fname; 

fname = new char[15]; 

// delta t 
double dt=0.0; 

// step counter 
int step =1; 

// stream to write file 
ofstream result; 
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// quaternions 

Quaternion drawQuat ('' d", 0.0) ; 

Quaternion qRotation("q”,0.0); 

// filter 

Qaef myQaef(l, 40, 3); 

cerr«"Enter file name, with .dat : " ; 
cin»fnaIne; 

writeFile(result,fname); 

// start filter and get delta t 
dt = myQaef.start 0; 

while(true){ 

// get estimated result 
drawQuat = myQaef.getResult(); 

// convert into angles 

qRotation = drawQuat.toAxisAngles(); 

/ /cerr«qRotation. getW ()«" " ; 

// write results in to file 

result« (step*dt)«" ’'«qRotation«endl; 

step++; 

} 

// stop the filter 
myQaef.stop(); 

return {0); 

} II end main () 

// end of file test.cpp 
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